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

Dependents:   Quaternion HAPS_GPS_Test_0002 GYSFDMAXB HAPS_EKF ... more

Revision:
0:deef87aa67a3
Child:
1:8e112552bdfc
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector3.hpp	Wed Jun 07 00:56:18 2017 +0000
@@ -0,0 +1,217 @@
+#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
\ No newline at end of file