<commit_before>#ifndef INCLUDE_AL_COORDINATEFRAME_HPP
#define INCLUDE_AL_COORDINATEFRAME_HPP

#include "math/al_Vec.hpp"
#include "math/al_Quat.hpp"


namespace al {

///<	A coordinate frame
///		Combines a Vec3d position with a Quat orientation
class Pose {
public:
	Pose(const Vec3d &v=Vec3d(0)): mVec(v) { mQuat.identity(); }
	
	Vec3d& pos(){ return mVec; }
	const Vec3d& pos() const { return mVec; }
	
	template <class T>
	Pose& pos(const Vec3<T>& v){ return vec(v); }
	
	Vec3d& vec(){ return mVec; }
	const Vec3d& vec() const { return mVec; }

	template <class T>
	Pose& vec(const Vec3<T>& v){ mVec.set(v); return *this; }

	Quatd& quat(){ return mQuat; }
	const Quatd& quat() const { return mQuat; }

	template <class T>
	Pose& quat(const Quat<T>& v){
		mQuat[0]=v[0]; mQuat[1]=v[1]; mQuat[2]=v[2]; mQuat[3]=v[3];
		return *this;
	}
	
	// get the azimuth, elevation & distance from this to another point
	void toAED(const Vec3d& to, double& azimuth, double& elevation, double& distance) const;

	// TODO: conversion operators for Pose->Vec3d, Pose->Quatd?	
protected:
	Vec3d mVec;		// position in 3-space
	Quatd mQuat;	// orientation of reference frame as a quaternion (relative to global axes)
};


///<	A mobile coordinate frame
///		A Pose that knows how to accumulate velocities
/// Smooth navigation with adjustable linear and angular velocity
class Nav : public Pose {
public:

	Nav(const Vec3d &position = Vec3d(0), double smooth=0.85, double turnRate=2)
	:	Pose(position), mSmooth(smooth)
	{	updateUnitVectors(); }

	/// Get smoothing amount
	double smooth() const { return mSmooth; }

	const Vec3d& ux() const { return mUX; }
	const Vec3d& uy() const { return mUY; }
	const Vec3d& uz() const { return mUZ; }
	
	/// Get linear and angular velocities as a Pose
	Pose vel() const {
		Pose p;
		p.vec(mMove1);
		p.quat(Quatd::fromEuler(mSpin1[1], mSpin1[0], mSpin1[2]));
		return p;
	}

	/// Set smoothing amount [0,1)
	Nav& smooth(double v){ mSmooth=v; return *this; }
	
	
	void view(const Quatd & v) {
		quat(v);
		updateUnitVectors();
	}
	
	void turn(const Quatd & v) {
		v.toEuler(mSpin1);
	}

	/// Set linear velocity
	void move(double x, double y, double z) { moveX(x); moveY(y); moveZ(z); }
	void moveX(double v){ mMove0[0] = v; }
	void moveY(double v){ mMove0[1] = v; }
	void moveZ(double v){ mMove0[2] = v; }

	/// Accelerate
	void push(double x, double y, double z) { pushX(x); pushY(y); pushZ(z); }
	void pushX(double amount) { mMove0[0] += amount; }
	void pushY(double amount) { mMove0[1] += amount; }
	void pushZ(double amount) { mMove0[2] += amount; }

	/// Set angular velocity
	void spinX(double v){ mSpin0[0] = v; }
	void spinY(double v){ mSpin0[1] = v; }
	void spinZ(double v){ mSpin0[2] = v; }

	/// Turn by a single increment for one step
	void turnX(double v){ mTurn[0] = v; }
	void turnY(double v){ mTurn[1] = v; }
	void turnZ(double v){ mTurn[2] = v; }

	/// Stop moving and spinning
	Nav& halt(){ mMove0.set(0); mSpin0.set(0); return *this; }

	/// Go to origin and reset orientation to identity
	Nav& home(){ 
		quat().identity();
		vec().set(0);
		updateUnitVectors();
		return *this;
	}

	/// Update coordinate frame basis vectors based on internal quaternion
	void updateUnitVectors(){
		quat().normalize();
		quat().toVectorX(mUX);
		quat().toVectorY(mUY);<commit_msg>updates<commit_after>#ifndef INCLUDE_AL_COORDINATEFRAME_HPP
#define INCLUDE_AL_COORDINATEFRAME_HPP

#include "math/al_Vec.hpp"
#include "math/al_Quat.hpp"


namespace al {

///<	A coordinate frame
///		Combines a Vec3d position with a Quat orientation
class Pose {
public:
	Pose(const Vec3d &v=Vec3d(0)): mVec(v) { mQuat.identity(); }
	
	Vec3d& pos(){ return mVec; }
	const Vec3d& pos() const { return mVec; }
	
	template <class T>
	Pose& pos(const Vec3<T>& v){ return vec(v); }
	
	Vec3d& vec(){ return mVec; }
	const Vec3d& vec() const { return mVec; }

	template <class T>
	Pose& vec(const Vec3<T>& v){ mVec.set(v); return *this; }

	Quatd& quat(){ return mQuat; }
	const Quatd& quat() const { return mQuat; }

	template <class T>
	Pose& quat(const Quat<T>& v){
		mQuat[0]=v[0]; mQuat[1]=v[1]; mQuat[2]=v[2]; mQuat[3]=v[3];
		return *this;
	}
	
	// get the azimuth, elevation & distance from this to another point
	void toAED(const Vec3d& to, double& azimuth, double& elevation, double& distance) const;

	// TODO: conversion operators for Pose->Vec3d, Pose->Quatd?	
protected:
	Vec3d mVec;		// position in 3-space
	Quatd mQuat;	// orientation of reference frame as a quaternion (relative to global axes)
};


///<	A mobile coordinate frame
///		A Pose that knows how to accumulate velocities
/// Smooth navigation with adjustable linear and angular velocity
class Nav : public Pose {
public:

	Nav(const Vec3d &position = Vec3d(0), double smooth=1)
	:	Pose(position), mSmooth(smooth)
	{	updateUnitVectors(); }

	/// Get smoothing amount
	double smooth() const { return mSmooth; }

	const Vec3d& ux() const { return mUX; }
	const Vec3d& uy() const { return mUY; }
	const Vec3d& uz() const { return mUZ; }
	
	/// Get linear and angular velocities as a Pose
	Pose vel() const {
		Pose p;
		p.vec(mMove1);
		p.quat(Quatd::fromEuler(mSpin1[1], mSpin1[0], mSpin1[2]));
		return p;
	}

	/// Set smoothing amount [0,1)
	Nav& smooth(double v){ mSmooth=v; return *this; }
	
	
	void view(const Quatd & v) {
		quat(v);
		updateUnitVectors();
	}
	
	void turn(const Quatd & v) {
		v.toEuler(mSpin1);
	}
	
	void position(double x, double y, double z) { mVec[0]=x; mVec[1]=y; mVec[2]=z; }
	void position(const Vec3d& p) { mVec.set(p); }

	/// Set linear velocity
	void move(double x, double y, double z) { moveX(x); moveY(y); moveZ(z); }
	void moveX(double v){ mMove0[0] = v; }
	void moveY(double v){ mMove0[1] = v; }
	void moveZ(double v){ mMove0[2] = v; }

	/// Accelerate
	void push(double x, double y, double z) { pushX(x); pushY(y); pushZ(z); }
	void pushX(double amount) { mMove0[0] += amount; }
	void pushY(double amount) { mMove0[1] += amount; }
	void pushZ(double amount) { mMove0[2] += amount; }

	/// Set angular velocity
	void spinX(double v){ mSpin0[0] = v; }
	void spinY(double v){ mSpin0[1] = v; }
	void spinZ(double v){ mSpin0[2] = v; }
	void spin(double a, double e, double b){ spinX(e); spinY(a); spinZ(b); }

	/// Turn by a single increment for one step
	void turnX(double v){ mTurn[0] = v; }
	void turnY(double v){ mTurn[1] = v; }
	void turnZ(double v){ mTurn[2] = v; }
	void turn(double a, double e, double b){ turnX(e); turnY(a); turnZ(b); }

	/// Stop moving and spinning
	Nav& halt(){ mMove0.set(0); mSpin0.set(0); quat().identity(); return *this; }

	/// Go to origin and reset orientation to identity
	Nav& home(){ 
		quat().identity();
		turn(0, 0, 0); 
		spin(0, 0, 0);
		vec().set(0);
		updateUnitVectors();
		return *this;
	}

	/// Update coordinate frame basis vectors based on internal quaternion
	void updateUnitVectors(){
		quat().normalize();
		quat().toVectorX(mUX);
		quat().toVectorY(mUY);
{'fraction_non_alphanumeric': 0.1571800057786767, 'fraction_numerical': 0.020658769141866512, 'mean_word_length': 3.9274021352313166, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 19, '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': '395963', 'n_tokens_mistral': 2845, 'n_tokens_neox': 2668, 'n_words': 785}