Gaku Matsumoto / Vector3

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

Dependents:   Quaternion HAPS_GPS_Test_0002 GYSFDMAXB HAPS_EKF ... more

Vector3.hpp@2:33c104711999, 2017-10-25 (annotated)

Committer:
Gaku0606
Date:
Wed Oct 25 08:08:00 2017 +0000
Revision:
2:33c104711999
Parent:
1:8e112552bdfc 
huhuhu

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 2:33c104711999 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 2:33c104711999 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 2:33c104711999 92
Gaku0606 2:33c104711999 93 /**
Gaku0606 2:33c104711999 94 * @bref ２つのVector3クラスのなす角を計算します.
Gaku0606 2:33c104711999 95 * @param 自分とのなす角度を計算するVector3クラスのインスタンス
Gaku0606 2:33c104711999 96 * @return 自分 * 相手となるベクトルを回転軸として符号付きのなす角度です
Gaku0606 2:33c104711999 97 */
Gaku0606 2:33c104711999 98 double sgnAngle(Vector3 vec);
Gaku0606 0:deef87aa67a3 99
Gaku0606 0:deef87aa67a3 100 /**
Gaku0606 1:8e112552bdfc 101 * @bref ゼロベクトルかどうか判定します.
Gaku0606 1:8e112552bdfc 102 * @note 0除法を防止するのに使ってください.
Gaku0606 1:8e112552bdfc 103 * @return 1ならゼロベクトル、0ならゼロベクトルではありません.
Gaku0606 0:deef87aa67a3 104 */
Gaku0606 0:deef87aa67a3 105 int CheckZero(){
Gaku0606 0:deef87aa67a3 106 if (x == 0.0 && y == 0.0 && z == 0.0){
Gaku0606 0:deef87aa67a3 107 return 1;
Gaku0606 0:deef87aa67a3 108 }
Gaku0606 0:deef87aa67a3 109 return 0;
Gaku0606 0:deef87aa67a3 110 }
Gaku0606 0:deef87aa67a3 111
Gaku0606 0:deef87aa67a3 112 /**
Gaku0606 1:8e112552bdfc 113 * @bref 自身のノルムを計算して返します.
Gaku0606 0:deef87aa67a3 114 */
Gaku0606 0:deef87aa67a3 115 double Norm();
Gaku0606 0:deef87aa67a3 116
Gaku0606 0:deef87aa67a3 117 /**
Gaku0606 1:8e112552bdfc 118 * @bref 単位ベクトルにします
Gaku0606 0:deef87aa67a3 119 */
Gaku0606 0:deef87aa67a3 120 void Normalize(){
Gaku0606 0:deef87aa67a3 121 double norm = sqrt(x*x + y*y + z*z);
Gaku0606 0:deef87aa67a3 122 if (norm != 0.0){
Gaku0606 0:deef87aa67a3 123 x /= norm;
Gaku0606 0:deef87aa67a3 124 y /= norm;
Gaku0606 0:deef87aa67a3 125 z /= norm;
Gaku0606 0:deef87aa67a3 126 return;
Gaku0606 0:deef87aa67a3 127 }
Gaku0606 0:deef87aa67a3 128 else{
Gaku0606 0:deef87aa67a3 129 return;
Gaku0606 0:deef87aa67a3 130 }
Gaku0606 0:deef87aa67a3 131 }
Gaku0606 0:deef87aa67a3 132 };
Gaku0606 0:deef87aa67a3 133
Gaku0606 0:deef87aa67a3 134 /**
Gaku0606 1:8e112552bdfc 135 * @bref Vector3クラスの各要素の和を計算します。
Gaku0606 0:deef87aa67a3 136 */
Gaku0606 0:deef87aa67a3 137 inline Vector3 operator+(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 138 static Vector3 vec;
Gaku0606 0:deef87aa67a3 139 vec.x = left.x + right.x;
Gaku0606 0:deef87aa67a3 140 vec.y = left.y + right.y;
Gaku0606 0:deef87aa67a3 141 vec.z = left.z + right.z;
Gaku0606 0:deef87aa67a3 142 return vec;
Gaku0606 0:deef87aa67a3 143 }
Gaku0606 0:deef87aa67a3 144
Gaku0606 0:deef87aa67a3 145 /**
Gaku0606 1:8e112552bdfc 146 * @bref Vector3クラスの各要素の差を計算します。
Gaku0606 0:deef87aa67a3 147 */
Gaku0606 0:deef87aa67a3 148 inline Vector3 operator-(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 149 static Vector3 vec;
Gaku0606 0:deef87aa67a3 150 vec.x = left.x - right.x;
Gaku0606 0:deef87aa67a3 151 vec.y = left.y - right.y;
Gaku0606 0:deef87aa67a3 152 vec.z = left.z - right.z;
Gaku0606 0:deef87aa67a3 153 return vec;
Gaku0606 0:deef87aa67a3 154 }
Gaku0606 0:deef87aa67a3 155
Gaku0606 0:deef87aa67a3 156 /**
Gaku0606 1:8e112552bdfc 157 * @bref Vector3クラスの外積を計算します.
Gaku0606 1:8e112552bdfc 158 * @note 外積ですので順序に注意してください.
Gaku0606 0:deef87aa67a3 159 */
Gaku0606 0:deef87aa67a3 160 inline Vector3 operator*(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 161 static Vector3 vec;
Gaku0606 0:deef87aa67a3 162 vec.x = left.y * right.z - left.z * right.y;
Gaku0606 0:deef87aa67a3 163 vec.y = left.z * right.x - left.x * right.z;
Gaku0606 0:deef87aa67a3 164 vec.z = left.x * right.y - left.y * right.x;
Gaku0606 0:deef87aa67a3 165 return vec;
Gaku0606 0:deef87aa67a3 166 }
Gaku0606 0:deef87aa67a3 167
Gaku0606 0:deef87aa67a3 168 /**
Gaku0606 1:8e112552bdfc 169 * @bref 内積を計算します
Gaku0606 1:8e112552bdfc 170 * @note ドットが使えなかったので％になりました。許してヒヤシンス
Gaku0606 0:deef87aa67a3 171 */
Gaku0606 0:deef87aa67a3 172 inline double operator%(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 173 return (left.x * right.x + left.y * right.y + left.z * right.z);
Gaku0606 0:deef87aa67a3 174 }
Gaku0606 0:deef87aa67a3 175
Gaku0606 0:deef87aa67a3 176 /**
Gaku0606 1:8e112552bdfc 177 * @bref Vector3クラスの各要素をスカラー倍します
Gaku0606 0:deef87aa67a3 178 */
Gaku0606 0:deef87aa67a3 179 inline Vector3 operator*(double scalar, Vector3 vec3){
Gaku0606 0:deef87aa67a3 180 static Vector3 vec;
Gaku0606 0:deef87aa67a3 181 vec.x = scalar * vec3.x;
Gaku0606 0:deef87aa67a3 182 vec.y = scalar * vec3.y;
Gaku0606 0:deef87aa67a3 183 vec.z = scalar * vec3.z;
Gaku0606 0:deef87aa67a3 184 return vec;
Gaku0606 0:deef87aa67a3 185 }
Gaku0606 0:deef87aa67a3 186
Gaku0606 0:deef87aa67a3 187 /**
Gaku0606 1:8e112552bdfc 188 * @bref Vector3クラスの各要素をスカラー倍します
Gaku0606 0:deef87aa67a3 189 */
Gaku0606 0:deef87aa67a3 190 inline Vector3 operator*(Vector3 vec3, double scalar){
Gaku0606 0:deef87aa67a3 191 static Vector3 vec;
Gaku0606 0:deef87aa67a3 192 vec.x = scalar * vec3.x;
Gaku0606 0:deef87aa67a3 193 vec.y = scalar * vec3.y;
Gaku0606 0:deef87aa67a3 194 vec.z = scalar * vec3.z;
Gaku0606 0:deef87aa67a3 195 return vec;
Gaku0606 0:deef87aa67a3 196 }
Gaku0606 0:deef87aa67a3 197
Gaku0606 0:deef87aa67a3 198 /**
Gaku0606 1:8e112552bdfc 199 * @bref Vector3クラスの各要素をスカラーで割ります
Gaku0606 0:deef87aa67a3 200 */
Gaku0606 0:deef87aa67a3 201 inline Vector3 operator/(Vector3 vec3, double scalar){
Gaku0606 0:deef87aa67a3 202 static Vector3 vec;
Gaku0606 0:deef87aa67a3 203 vec.x = vec3.x / scalar;
Gaku0606 0:deef87aa67a3 204 vec.y = vec3.y / scalar;
Gaku0606 0:deef87aa67a3 205 vec.z = vec3.z / scalar;
Gaku0606 0:deef87aa67a3 206 return vec;
Gaku0606 0:deef87aa67a3 207 }
Gaku0606 0:deef87aa67a3 208
Gaku0606 0:deef87aa67a3 209 template<typename T>void Vector3::set(T _x, T _y, T _z){
Gaku0606 0:deef87aa67a3 210 x = _x;
Gaku0606 0:deef87aa67a3 211 y = _y;
Gaku0606 0:deef87aa67a3 212 z = _z;
Gaku0606 0:deef87aa67a3 213 }
Gaku0606 0:deef87aa67a3 214
Gaku0606 0:deef87aa67a3 215 inline double Vector3::Angle(Vector3 vec){
Gaku0606 0:deef87aa67a3 216 double r = (*this % vec) / (this->Norm() * vec.Norm());
Gaku0606 0:deef87aa67a3 217 return acos(r);
Gaku0606 0:deef87aa67a3 218 }
Gaku0606 0:deef87aa67a3 219
Gaku0606 2:33c104711999 220 inline double Vector3::sgnAngle(Vector3 vec){
Gaku0606 2:33c104711999 221 double theta = this->Angle(vec);
Gaku0606 2:33c104711999 222 vec = *this * vec;
Gaku0606 2:33c104711999 223 if(vec.z < 0) theta *= -1.0;
Gaku0606 2:33c104711999 224
Gaku0606 2:33c104711999 225 return theta;
Gaku0606 2:33c104711999 226 }
Gaku0606 2:33c104711999 227
Gaku0606 0:deef87aa67a3 228 inline double Vector3::Norm(){
Gaku0606 0:deef87aa67a3 229 return sqrt(x*x + y*y + z*z);
Gaku0606 0:deef87aa67a3 230 }
Gaku0606 0:deef87aa67a3 231
Gaku0606 0:deef87aa67a3 232 #endif