Gaku Matsumoto / Vector3

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

Dependents:   Quaternion HAPS_GPS_Test_0002 GYSFDMAXB HAPS_EKF ... more

Vector3.hpp@1:8e112552bdfc, 2017-06-09 (annotated)

Committer:
Gaku0606
Date:
Fri Jun 09 19:54:15 2017 +0000
Revision:
1:8e112552bdfc
Parent:
0:deef87aa67a3
Child:
2:33c104711999
Child:
3:3d5d3689521c
??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaku0606 0:deef87aa67a3 1 #include <math.h>
Gaku0606 0:deef87aa67a3 2 #include <stdio.h>
Gaku0606 0:deef87aa67a3 3 #include <stdlib.h>
Gaku0606 0:deef87aa67a3 4
Gaku0606 0:deef87aa67a3 5 #ifndef _VECTOR3_HPP_
Gaku0606 0:deef87aa67a3 6 #define _VECTOR3_HPP_
Gaku0606 0:deef87aa67a3 7
Gaku0606 0:deef87aa67a3 8 class Vector3{
Gaku0606 0:deef87aa67a3 9 public:
Gaku0606 0:deef87aa67a3 10 Vector3(){
Gaku0606 0:deef87aa67a3 11 x = 0.0;
Gaku0606 0:deef87aa67a3 12 y = 0.0;
Gaku0606 0:deef87aa67a3 13 z = 0.0;
Gaku0606 0:deef87aa67a3 14 };
Gaku0606 0:deef87aa67a3 15 Vector3(double _x, double _y, double _z){
Gaku0606 0:deef87aa67a3 16 x = _x;
Gaku0606 0:deef87aa67a3 17 y = _y;
Gaku0606 0:deef87aa67a3 18 z = _z;
Gaku0606 0:deef87aa67a3 19 };
Gaku0606 0:deef87aa67a3 20
Gaku0606 0:deef87aa67a3 21 public:
Gaku0606 0:deef87aa67a3 22 double x;
Gaku0606 0:deef87aa67a3 23 double y;
Gaku0606 0:deef87aa67a3 24 double z;
Gaku0606 0:deef87aa67a3 25
Gaku0606 0:deef87aa67a3 26 /**
Gaku0606 1:8e112552bdfc 27 * @bref ３次元ベクトルの要素をコピー
Gaku0606 0:deef87aa67a3 28 */
Gaku0606 0:deef87aa67a3 29 Vector3 operator=(Vector3 vector){
Gaku0606 0:deef87aa67a3 30 x = vector.x;
Gaku0606 0:deef87aa67a3 31 y = vector.y;
Gaku0606 0:deef87aa67a3 32 z = vector.z;
Gaku0606 0:deef87aa67a3 33 return *this;
Gaku0606 0:deef87aa67a3 34 };
Gaku0606 0:deef87aa67a3 35
Gaku0606 1:8e112552bdfc 36 /**３次元ベクトルを足して代入する
Gaku0606 0:deef87aa67a3 37 Vector3 operator+=(Vector3 vec){
Gaku0606 0:deef87aa67a3 38 x += vec.x;
Gaku0606 0:deef87aa67a3 39 y += vec.y;
Gaku0606 0:deef87aa67a3 40 z += vec.z;
Gaku0606 0:deef87aa67a3 41 return *this;
Gaku0606 0:deef87aa67a3 42 };
Gaku0606 0:deef87aa67a3 43
Gaku0606 1:8e112552bdfc 44 /**３次元ベクトルを引いて代入する*/
Gaku0606 0:deef87aa67a3 45 Vector3 operator-=(Vector3 vec){
Gaku0606 0:deef87aa67a3 46 x -= vec.x;
Gaku0606 0:deef87aa67a3 47 y -= vec.y;
Gaku0606 0:deef87aa67a3 48 z -= vec.z;
Gaku0606 0:deef87aa67a3 49 return *this;
Gaku0606 0:deef87aa67a3 50 };
Gaku0606 0:deef87aa67a3 51
Gaku0606 1:8e112552bdfc 52 /**３次元ベクトルを外積して代入する*/
Gaku0606 0:deef87aa67a3 53 Vector3 operator*=(Vector3 vec){
Gaku0606 0:deef87aa67a3 54 double _x = y*vec.z - z*vec.y;
Gaku0606 0:deef87aa67a3 55 double _y = z*vec.x - x*vec.z;
Gaku0606 0:deef87aa67a3 56 double _z = x*vec.y - y*vec.x;
Gaku0606 0:deef87aa67a3 57 x = _x;
Gaku0606 0:deef87aa67a3 58 y = _y;
Gaku0606 0:deef87aa67a3 59 z = _z;
Gaku0606 0:deef87aa67a3 60 return *this;
Gaku0606 0:deef87aa67a3 61 }
Gaku0606 0:deef87aa67a3 62
Gaku0606 0:deef87aa67a3 63 Vector3 operator/=(double scalar){
Gaku0606 0:deef87aa67a3 64 x /= scalar;
Gaku0606 0:deef87aa67a3 65 y /= scalar;
Gaku0606 0:deef87aa67a3 66 z /= scalar;
Gaku0606 0:deef87aa67a3 67 return *this;
Gaku0606 0:deef87aa67a3 68 }
Gaku0606 0:deef87aa67a3 69
Gaku0606 0:deef87aa67a3 70 bool operator==(Vector3 vec){
Gaku0606 0:deef87aa67a3 71 if ((x == vec.x) && (y == vec.y) && (z == vec.z)){
Gaku0606 0:deef87aa67a3 72 return true;
Gaku0606 0:deef87aa67a3 73 }
Gaku0606 0:deef87aa67a3 74 return false;
Gaku0606 0:deef87aa67a3 75 }
Gaku0606 0:deef87aa67a3 76 bool operator!=(Vector3 vec){
Gaku0606 0:deef87aa67a3 77 if ((x == vec.x) && (y == vec.y) && (z == vec.z)){
Gaku0606 0:deef87aa67a3 78 return false;
Gaku0606 0:deef87aa67a3 79 }
Gaku0606 0:deef87aa67a3 80 return true;
Gaku0606 0:deef87aa67a3 81 }
Gaku0606 0:deef87aa67a3 82 /**
Gaku0606 1:8e112552bdfc 83 * @bref Vector3クラスの各要素を初期化します。
Gaku0606 0:deef87aa67a3 84 */
Gaku0606 0:deef87aa67a3 85 template<typename T>void set(T _x, T _y, T _z);
Gaku0606 0:deef87aa67a3 86
Gaku0606 0:deef87aa67a3 87 /**
Gaku0606 1:8e112552bdfc 88 * @bref ２つのVector3クラスのなす角を計算します.
Gaku0606 1:8e112552bdfc 89 * @param 自分とのなす角度を計算するVector3クラスのインスタンス
Gaku0606 0:deef87aa67a3 90 */
Gaku0606 0:deef87aa67a3 91 double Angle(Vector3 v);
Gaku0606 0:deef87aa67a3 92
Gaku0606 0:deef87aa67a3 93 /**
Gaku0606 1:8e112552bdfc 94 * @bref ゼロベクトルかどうか判定します.
Gaku0606 1:8e112552bdfc 95 * @note 0除法を防止するのに使ってください.
Gaku0606 1:8e112552bdfc 96 * @return 1ならゼロベクトル、0ならゼロベクトルではありません.
Gaku0606 0:deef87aa67a3 97 */
Gaku0606 0:deef87aa67a3 98 int CheckZero(){
Gaku0606 0:deef87aa67a3 99 if (x == 0.0 && y == 0.0 && z == 0.0){
Gaku0606 0:deef87aa67a3 100 return 1;
Gaku0606 0:deef87aa67a3 101 }
Gaku0606 0:deef87aa67a3 102 return 0;
Gaku0606 0:deef87aa67a3 103 }
Gaku0606 0:deef87aa67a3 104
Gaku0606 0:deef87aa67a3 105 /**
Gaku0606 1:8e112552bdfc 106 * @bref 自身のノルムを計算して返します.
Gaku0606 0:deef87aa67a3 107 */
Gaku0606 0:deef87aa67a3 108 double Norm();
Gaku0606 0:deef87aa67a3 109
Gaku0606 0:deef87aa67a3 110 /**
Gaku0606 1:8e112552bdfc 111 * @bref 単位ベクトルにします
Gaku0606 0:deef87aa67a3 112 */
Gaku0606 0:deef87aa67a3 113 void Normalize(){
Gaku0606 0:deef87aa67a3 114 double norm = sqrt(x*x + y*y + z*z);
Gaku0606 0:deef87aa67a3 115 if (norm != 0.0){
Gaku0606 0:deef87aa67a3 116 x /= norm;
Gaku0606 0:deef87aa67a3 117 y /= norm;
Gaku0606 0:deef87aa67a3 118 z /= norm;
Gaku0606 0:deef87aa67a3 119 return;
Gaku0606 0:deef87aa67a3 120 }
Gaku0606 0:deef87aa67a3 121 else{
Gaku0606 0:deef87aa67a3 122 return;
Gaku0606 0:deef87aa67a3 123 }
Gaku0606 0:deef87aa67a3 124 }
Gaku0606 0:deef87aa67a3 125 };
Gaku0606 0:deef87aa67a3 126
Gaku0606 0:deef87aa67a3 127 /**
Gaku0606 1:8e112552bdfc 128 * @bref Vector3クラスの各要素の和を計算します。
Gaku0606 0:deef87aa67a3 129 */
Gaku0606 0:deef87aa67a3 130 inline Vector3 operator+(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 131 static Vector3 vec;
Gaku0606 0:deef87aa67a3 132 vec.x = left.x + right.x;
Gaku0606 0:deef87aa67a3 133 vec.y = left.y + right.y;
Gaku0606 0:deef87aa67a3 134 vec.z = left.z + right.z;
Gaku0606 0:deef87aa67a3 135 return vec;
Gaku0606 0:deef87aa67a3 136 }
Gaku0606 0:deef87aa67a3 137
Gaku0606 0:deef87aa67a3 138 /**
Gaku0606 1:8e112552bdfc 139 * @bref Vector3クラスの各要素の差を計算します。
Gaku0606 0:deef87aa67a3 140 */
Gaku0606 0:deef87aa67a3 141 inline Vector3 operator-(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 142 static Vector3 vec;
Gaku0606 0:deef87aa67a3 143 vec.x = left.x - right.x;
Gaku0606 0:deef87aa67a3 144 vec.y = left.y - right.y;
Gaku0606 0:deef87aa67a3 145 vec.z = left.z - right.z;
Gaku0606 0:deef87aa67a3 146 return vec;
Gaku0606 0:deef87aa67a3 147 }
Gaku0606 0:deef87aa67a3 148
Gaku0606 0:deef87aa67a3 149 /**
Gaku0606 1:8e112552bdfc 150 * @bref Vector3クラスの外積を計算します.
Gaku0606 1:8e112552bdfc 151 * @note 外積ですので順序に注意してください.
Gaku0606 0:deef87aa67a3 152 */
Gaku0606 0:deef87aa67a3 153 inline Vector3 operator*(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 154 static Vector3 vec;
Gaku0606 0:deef87aa67a3 155 vec.x = left.y * right.z - left.z * right.y;
Gaku0606 0:deef87aa67a3 156 vec.y = left.z * right.x - left.x * right.z;
Gaku0606 0:deef87aa67a3 157 vec.z = left.x * right.y - left.y * right.x;
Gaku0606 0:deef87aa67a3 158 return vec;
Gaku0606 0:deef87aa67a3 159 }
Gaku0606 0:deef87aa67a3 160
Gaku0606 0:deef87aa67a3 161 /**
Gaku0606 1:8e112552bdfc 162 * @bref 内積を計算します
Gaku0606 1:8e112552bdfc 163 * @note ドットが使えなかったので％になりました。許してヒヤシンス
Gaku0606 0:deef87aa67a3 164 */
Gaku0606 0:deef87aa67a3 165 inline double operator%(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 166 return (left.x * right.x + left.y * right.y + left.z * right.z);
Gaku0606 0:deef87aa67a3 167 }
Gaku0606 0:deef87aa67a3 168
Gaku0606 0:deef87aa67a3 169 /**
Gaku0606 1:8e112552bdfc 170 * @bref Vector3クラスの各要素をスカラー倍します
Gaku0606 0:deef87aa67a3 171 */
Gaku0606 0:deef87aa67a3 172 inline Vector3 operator*(double scalar, Vector3 vec3){
Gaku0606 0:deef87aa67a3 173 static Vector3 vec;
Gaku0606 0:deef87aa67a3 174 vec.x = scalar * vec3.x;
Gaku0606 0:deef87aa67a3 175 vec.y = scalar * vec3.y;
Gaku0606 0:deef87aa67a3 176 vec.z = scalar * vec3.z;
Gaku0606 0:deef87aa67a3 177 return vec;
Gaku0606 0:deef87aa67a3 178 }
Gaku0606 0:deef87aa67a3 179
Gaku0606 0:deef87aa67a3 180 /**
Gaku0606 1:8e112552bdfc 181 * @bref Vector3クラスの各要素をスカラー倍します
Gaku0606 0:deef87aa67a3 182 */
Gaku0606 0:deef87aa67a3 183 inline Vector3 operator*(Vector3 vec3, double scalar){
Gaku0606 0:deef87aa67a3 184 static Vector3 vec;
Gaku0606 0:deef87aa67a3 185 vec.x = scalar * vec3.x;
Gaku0606 0:deef87aa67a3 186 vec.y = scalar * vec3.y;
Gaku0606 0:deef87aa67a3 187 vec.z = scalar * vec3.z;
Gaku0606 0:deef87aa67a3 188 return vec;
Gaku0606 0:deef87aa67a3 189 }
Gaku0606 0:deef87aa67a3 190
Gaku0606 0:deef87aa67a3 191 /**
Gaku0606 1:8e112552bdfc 192 * @bref Vector3クラスの各要素をスカラーで割ります
Gaku0606 0:deef87aa67a3 193 */
Gaku0606 0:deef87aa67a3 194 inline Vector3 operator/(Vector3 vec3, double scalar){
Gaku0606 0:deef87aa67a3 195 static Vector3 vec;
Gaku0606 0:deef87aa67a3 196 vec.x = vec3.x / scalar;
Gaku0606 0:deef87aa67a3 197 vec.y = vec3.y / scalar;
Gaku0606 0:deef87aa67a3 198 vec.z = vec3.z / scalar;
Gaku0606 0:deef87aa67a3 199 return vec;
Gaku0606 0:deef87aa67a3 200 }
Gaku0606 0:deef87aa67a3 201
Gaku0606 0:deef87aa67a3 202 template<typename T>void Vector3::set(T _x, T _y, T _z){
Gaku0606 0:deef87aa67a3 203 x = _x;
Gaku0606 0:deef87aa67a3 204 y = _y;
Gaku0606 0:deef87aa67a3 205 z = _z;
Gaku0606 0:deef87aa67a3 206 }
Gaku0606 0:deef87aa67a3 207
Gaku0606 0:deef87aa67a3 208 inline double Vector3::Angle(Vector3 vec){
Gaku0606 0:deef87aa67a3 209 double r = (*this % vec) / (this->Norm() * vec.Norm());
Gaku0606 0:deef87aa67a3 210 return acos(r);
Gaku0606 0:deef87aa67a3 211 }
Gaku0606 0:deef87aa67a3 212
Gaku0606 0:deef87aa67a3 213 inline double Vector3::Norm(){
Gaku0606 0:deef87aa67a3 214 return sqrt(x*x + y*y + z*z);
Gaku0606 0:deef87aa67a3 215 }
Gaku0606 0:deef87aa67a3 216
Gaku0606 0:deef87aa67a3 217 #endif