3次元ベクトルクラスVector3ライブラリです。
Dependents: Quaternion HAPS_GPS_Test_0002 GYSFDMAXB HAPS_EKF ... more
Vector3.hpp
- Committer:
- Gaku0606
- Date:
- 2017-10-25
- Revision:
- 2:33c104711999
- Parent:
- 1:8e112552bdfc
File content as of revision 2:33c104711999:
#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 3次元ベクトルの要素をコピー */ Vector3 operator=(Vector3 vector){ x = vector.x; y = vector.y; z = vector.z; return *this; }; /**3次元ベクトルを足して代入する*/ Vector3 operator+=(Vector3 vec){ x += vec.x; y += vec.y; z += vec.z; return *this; }; /** 3次元ベクトルを引いて代入する */ Vector3 operator-=(Vector3 vec){ x -= vec.x; y -= vec.y; z -= vec.z; return *this; }; /**3次元ベクトルを外積して代入する*/ 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クラスの各要素を初期化します。 */ template<typename T>void set(T _x, T _y, T _z); /** * @bref 2つのVector3クラスのなす角を計算します. * @param 自分とのなす角度を計算するVector3クラスのインスタンス */ double Angle(Vector3 v); /** * @bref 2つのVector3クラスのなす角を計算します. * @param 自分とのなす角度を計算するVector3クラスのインスタンス * @return 自分 * 相手となるベクトルを回転軸として符号付きのなす角度です */ double sgnAngle(Vector3 vec); /** * @bref ゼロベクトルかどうか判定します. * @note 0除法を防止するのに使ってください. * @return 1ならゼロベクトル、0ならゼロベクトルではありません. */ int CheckZero(){ if (x == 0.0 && y == 0.0 && z == 0.0){ return 1; } return 0; } /** * @bref 自身のノルムを計算して返します. */ double Norm(); /** * @bref 単位ベクトルにします */ 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クラスの各要素の和を計算します。 */ 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クラスの各要素の差を計算します。 */ 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クラスの外積を計算します. * @note 外積ですので順序に注意してください. */ 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 内積を計算します * @note ドットが使えなかったので%になりました。許してヒヤシンス */ inline double operator%(Vector3 left, Vector3 right){ return (left.x * right.x + left.y * right.y + left.z * right.z); } /** * @bref Vector3クラスの各要素をスカラー倍します */ 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クラスの各要素をスカラー倍します */ 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クラスの各要素をスカラーで割ります */ 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::sgnAngle(Vector3 vec){ double theta = this->Angle(vec); vec = *this * vec; if(vec.z < 0) theta *= -1.0; return theta; } inline double Vector3::Norm(){ return sqrt(x*x + y*y + z*z); } #endif