3次元ベクトルクラスVector3ライブラリです。
Dependents: Quaternion HAPS_GPS_Test_0002 GYSFDMAXB HAPS_EKF ... more
Diff: Vector3.hpp
- Revision:
- 0:deef87aa67a3
- Child:
- 1:8e112552bdfc
diff -r 000000000000 -r deef87aa67a3 Vector3.hpp --- /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³xNgÌvfðRs[ + */ + Vector3 operator=(Vector3 vector){ + x = vector.x; + y = vector.y; + z = vector.z; + return *this; + }; + + /** R³xNgð«µÄãü·é*/ + Vector3 operator+=(Vector3 vec){ + x += vec.x; + y += vec.y; + z += vec.z; + return *this; + }; + + /** R³xNgðø¢Äãü·é*/ + Vector3 operator-=(Vector3 vec){ + x -= vec.x; + y -= vec.y; + z -= vec.z; + return *this; + }; + + /** R³xNgð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 Vector3NXÌevfðú»µÜ·B + */ + template<typename T>void set(T _x, T _y, T _z); + + /** + * @bref QÂÌVector3NXÌÈ·pðvZµÜ·. + * @param ©ªÆÌÈ·pxðvZ·éVector3NXÌCX^X + */ + double Angle(Vector3 v); + + /** + * @bref [xNg©Ç¤©»èµÜ·. + * @note 0@ðh~·éÌÉgÁľ³¢. + * @return 1Èç[xNgA0Èç[xNgÅÍ èܹñ. + */ + int CheckZero(){ + if (x == 0.0 && y == 0.0 && z == 0.0){ + return 1; + } + return 0; + } + + /** + * @bref ©gÌmðvZµÄԵܷ. + */ + double Norm(); + + /** + * @bref PÊxNgɵܷ + */ + 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 Vector3NXÌevfÌaðvZµÜ·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 Vector3NXÌevfÌ·ðvZµÜ·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 Vector3NXÌOÏðvZµÜ·. +* @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 àÏðvZµÜ· +* @note hbgªg¦È©Á½ÌÅÉÈèܵ½BµÄqVX +*/ +inline double operator%(Vector3 left, Vector3 right){ + return (left.x * right.x + left.y * right.y + left.z * right.z); +} + +/** +* @bref Vector3NXÌevfðXJ[{µÜ· +*/ +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 Vector3NXÌevfðXJ[{µÜ· +*/ +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 Vector3NXÌevfðXJ[ÅèÜ· +*/ +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