3次元ベクトルクラスVector3ライブラリです。

Dependents:   Quaternion HAPS_GPS_Test_0002 GYSFDMAXB HAPS_EKF ... more

Vector3.hpp

Committer:
Gaku0606
Date:
2017-06-07
Revision:
0:deef87aa67a3
Child:
1:8e112552bdfc

File content as of revision 0:deef87aa67a3:

#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#ifndef _VECTOR3_HPP_
#define _VECTOR3_HPP_

class Vector3{
public:
	Vector3(){
		x = 0.0;
		y = 0.0;
		z = 0.0;
	};
	Vector3(double _x, double _y, double _z){
		x = _x;
		y = _y;
		z = _z;
	};

public:
	double x;
	double y;
	double z;

	/**
	*  @bref ‚RŽŸŒ³ƒxƒNƒgƒ‹‚Ì—v‘f‚ðƒRƒs[
	*/
	Vector3 operator=(Vector3 vector){
		x = vector.x;
		y = vector.y;
		z = vector.z;
		return *this;
	};

	/** ‚RŽŸŒ³ƒxƒNƒgƒ‹‚ð‘«‚µ‚Ä‘ã“ü‚·‚é*/
	Vector3 operator+=(Vector3 vec){
		x += vec.x;
		y += vec.y;
		z += vec.z;
		return *this;
	};

	/** ‚RŽŸŒ³ƒxƒNƒgƒ‹‚ðˆø‚¢‚Ä‘ã“ü‚·‚é*/
	Vector3 operator-=(Vector3 vec){
		x -= vec.x;
		y -= vec.y;
		z -= vec.z;
		return *this;
	};

	/** ‚RŽŸŒ³ƒxƒNƒgƒ‹‚ðŠOÏ‚µ‚Ä‘ã“ü‚·‚é*/
	Vector3 operator*=(Vector3 vec){
		double _x = y*vec.z - z*vec.y;
		double _y = z*vec.x - x*vec.z;
		double _z = x*vec.y - y*vec.x;
		x = _x;
		y = _y;
		z = _z;
		return *this;
	}

	Vector3 operator/=(double scalar){
		x /= scalar;
		y /= scalar;
		z /= scalar;
		return *this;
	}

	bool operator==(Vector3 vec){
		if ((x == vec.x) && (y == vec.y) && (z == vec.z)){
			return true;
		}
		return false;
	}
	bool operator!=(Vector3 vec){
		if ((x == vec.x) && (y == vec.y) && (z == vec.z)){
			return false;
		}
		return true;
	}
	/**
	*  @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚ð‰Šú‰»‚µ‚Ü‚·B
	*/
	template<typename T>void set(T _x, T _y, T _z);

	/**
	*  @bref ‚Q‚‚ÌVector3ƒNƒ‰ƒX‚Ì‚È‚·Šp‚ðŒvŽZ‚µ‚Ü‚·.
	*  @param Ž©•ª‚Æ‚Ì‚È‚·Šp“x‚ðŒvŽZ‚·‚éVector3ƒNƒ‰ƒX‚̃Cƒ“ƒXƒ^ƒ“ƒX
	*/
	double Angle(Vector3 v);

	/**
	*  @bref ƒ[ƒƒxƒNƒgƒ‹‚©‚Ç‚¤‚©”»’肵‚Ü‚·.
	*  @note 0œ–@‚ð–hŽ~‚·‚é‚Ì‚ÉŽg‚Á‚Ä‚­‚¾‚³‚¢.
	*  @return 1‚È‚çƒ[ƒƒxƒNƒgƒ‹A0‚È‚çƒ[ƒƒxƒNƒgƒ‹‚Å‚Í‚ ‚è‚Ü‚¹‚ñ.
	*/
	int CheckZero(){
		if (x == 0.0 && y == 0.0 && z == 0.0){
			return 1;
		}
		return 0;
	}

	/**
	*  @bref Ž©g‚̃mƒ‹ƒ€‚ðŒvŽZ‚µ‚Ä•Ô‚µ‚Ü‚·.
	*/
	double Norm();

	/**
	*  @bref  ’PˆÊƒxƒNƒgƒ‹‚É‚µ‚Ü‚·
	*/
	void Normalize(){
		double norm = sqrt(x*x + y*y + z*z);
		if (norm != 0.0){
			x /= norm;
			y /= norm;
			z /= norm;
			return;
		}
		else{
			return;
		}
	}
};

/**
*  @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚̘a‚ðŒvŽZ‚µ‚Ü‚·B
*/
inline Vector3 operator+(Vector3 left, Vector3 right){
	static Vector3 vec;
	vec.x = left.x + right.x;
	vec.y = left.y + right.y;
	vec.z = left.z + right.z;
	return vec;
}

/**
* @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚̍·‚ðŒvŽZ‚µ‚Ü‚·B
*/
inline Vector3 operator-(Vector3 left, Vector3 right){
	static Vector3 vec;
	vec.x = left.x - right.x;
	vec.y = left.y - right.y;
	vec.z = left.z - right.z;
	return vec;
}

/**
*  @bref Vector3ƒNƒ‰ƒX‚ÌŠOÏ‚ðŒvŽZ‚µ‚Ü‚·.
*  @note ŠOÏ‚Å‚·‚̂ŏ‡˜‚É’ˆÓ‚µ‚Ä‚­‚¾‚³‚¢.
*/
inline Vector3 operator*(Vector3 left, Vector3 right){
	static Vector3 vec;
	vec.x = left.y * right.z - left.z * right.y;
	vec.y = left.z * right.x - left.x * right.z;
	vec.z = left.x * right.y - left.y * right.x;
	return vec;
}

/**
*  @bref “àÏ‚ðŒvŽZ‚µ‚Ü‚·
*  @note ƒhƒbƒg‚ªŽg‚¦‚È‚©‚Á‚½‚̂Ł“‚É‚È‚è‚Ü‚µ‚½B‹–‚µ‚ăqƒ„ƒVƒ“ƒX
*/
inline double operator%(Vector3 left, Vector3 right){
	return (left.x * right.x + left.y * right.y + left.z * right.z);
}

/**
*  @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚ðƒXƒJƒ‰[”{‚µ‚Ü‚·
*/
inline Vector3 operator*(double scalar, Vector3 vec3){
	static Vector3 vec;
	vec.x = scalar * vec3.x;
	vec.y = scalar * vec3.y;
	vec.z = scalar * vec3.z;
	return vec;
}

/**
*  @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚ðƒXƒJƒ‰[”{‚µ‚Ü‚·
*/
inline Vector3 operator*(Vector3 vec3, double scalar){
	static Vector3 vec;
	vec.x = scalar * vec3.x;
	vec.y = scalar * vec3.y;
	vec.z = scalar * vec3.z;
	return vec;
}

/**
*  @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚ðƒXƒJƒ‰[‚ÅŠ„‚è‚Ü‚·
*/
inline Vector3 operator/(Vector3 vec3, double scalar){
	static Vector3 vec;
	vec.x = vec3.x / scalar;
	vec.y = vec3.y / scalar;
	vec.z = vec3.z / scalar;
	return vec;
}

template<typename T>void Vector3::set(T _x, T _y, T _z){
	x = _x;
	y = _y;
	z = _z;
}

inline double Vector3::Angle(Vector3 vec){
	double r = (*this % vec) / (this->Norm() * vec.Norm());
	return acos(r);
}

inline double Vector3::Norm(){
	return sqrt(x*x + y*y + z*z);
}

#endif