3次元ベクトルクラス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?
User | Revision | Line number | New 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 3次元ベクトルの要素をコピー |
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 | /**3次元ベクトルを足して代入する*/ |
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 | /** 3次元ベクトルを引いて代入する */ |
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 | /**3次元ベクトルを外積して代入する*/ |
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 2つの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 2つの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 |