Gaku Matsumoto / Vector3

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

Dependents:   Quaternion HAPS_GPS_Test_0002 GYSFDMAXB HAPS_EKF ... more

Vector3.hpp@4:d68f06f4d554, 2018-02-25 (annotated)

Committer:
Gaku0606
Date:
Sun Feb 25 19:44:38 2018 +0000
Revision:
4:d68f06f4d554
Parent:
3:3d5d3689521c 
aaa;

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 3:3d5d3689521c 11 x = 0.0f;
Gaku0606 3:3d5d3689521c 12 y = 0.0f;
Gaku0606 3:3d5d3689521c 13 z = 0.0f;
Gaku0606 0:deef87aa67a3 14 };
Gaku0606 3:3d5d3689521c 15 Vector3(float _x, float _y, float _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 3:3d5d3689521c 22 float x;
Gaku0606 3:3d5d3689521c 23 float y;
Gaku0606 3:3d5d3689521c 24 float 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 3:3d5d3689521c 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 3:3d5d3689521c 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 3:3d5d3689521c 54 float _x = y*vec.z - z*vec.y;
Gaku0606 3:3d5d3689521c 55 float _y = z*vec.x - x*vec.z;
Gaku0606 3:3d5d3689521c 56 float _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 3:3d5d3689521c 63 Vector3 operator/=(float 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 3:3d5d3689521c 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 3:3d5d3689521c 91 float Angle(Vector3 v);
Gaku0606 3:3d5d3689521c 92
Gaku0606 3:3d5d3689521c 93 /**
Gaku0606 3:3d5d3689521c 94 * @bref ２つのVector3クラスのなす角を計算します.
Gaku0606 3:3d5d3689521c 95 * @param 自分とのなす角度を計算するVector3クラスのインスタンス
Gaku0606 3:3d5d3689521c 96 * @return 自分 * 相手となるベクトルを回転軸として符号付きのなす角度です
Gaku0606 3:3d5d3689521c 97 */
Gaku0606 3:3d5d3689521c 98 float 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 3:3d5d3689521c 106 if (x == 0.0f && y == 0.0f && z == 0.0f){
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 3:3d5d3689521c 115 float 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 4:d68f06f4d554 121 float norm = sqrtf(x*x + y*y + z*z);
Gaku0606 3:3d5d3689521c 122 if (norm != 0.0f){
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 3:3d5d3689521c 132
Gaku0606 4:d68f06f4d554 133 char* toString() {
Gaku0606 4:d68f06f4d554 134 char str[128] = {};
Gaku0606 4:d68f06f4d554 135
Gaku0606 4:d68f06f4d554 136 sprintf(str, "(%f,%f,%f)", x, y, z);
Gaku0606 4:d68f06f4d554 137 return str;
Gaku0606 4:d68f06f4d554 138 }
Gaku0606 3:3d5d3689521c 139
Gaku0606 0:deef87aa67a3 140 };
Gaku0606 0:deef87aa67a3 141
Gaku0606 0:deef87aa67a3 142 /**
Gaku0606 1:8e112552bdfc 143 * @bref Vector3クラスの各要素の和を計算します。
Gaku0606 0:deef87aa67a3 144 */
Gaku0606 0:deef87aa67a3 145 inline Vector3 operator+(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 146 static Vector3 vec;
Gaku0606 0:deef87aa67a3 147 vec.x = left.x + right.x;
Gaku0606 0:deef87aa67a3 148 vec.y = left.y + right.y;
Gaku0606 0:deef87aa67a3 149 vec.z = left.z + right.z;
Gaku0606 0:deef87aa67a3 150 return vec;
Gaku0606 0:deef87aa67a3 151 }
Gaku0606 0:deef87aa67a3 152
Gaku0606 0:deef87aa67a3 153 /**
Gaku0606 1:8e112552bdfc 154 * @bref Vector3クラスの各要素の差を計算します。
Gaku0606 0:deef87aa67a3 155 */
Gaku0606 0:deef87aa67a3 156 inline Vector3 operator-(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 157 static Vector3 vec;
Gaku0606 0:deef87aa67a3 158 vec.x = left.x - right.x;
Gaku0606 0:deef87aa67a3 159 vec.y = left.y - right.y;
Gaku0606 0:deef87aa67a3 160 vec.z = left.z - right.z;
Gaku0606 0:deef87aa67a3 161 return vec;
Gaku0606 0:deef87aa67a3 162 }
Gaku0606 0:deef87aa67a3 163
Gaku0606 0:deef87aa67a3 164 /**
Gaku0606 1:8e112552bdfc 165 * @bref Vector3クラスの外積を計算します.
Gaku0606 1:8e112552bdfc 166 * @note 外積ですので順序に注意してください.
Gaku0606 0:deef87aa67a3 167 */
Gaku0606 0:deef87aa67a3 168 inline Vector3 operator*(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 169 static Vector3 vec;
Gaku0606 0:deef87aa67a3 170 vec.x = left.y * right.z - left.z * right.y;
Gaku0606 0:deef87aa67a3 171 vec.y = left.z * right.x - left.x * right.z;
Gaku0606 0:deef87aa67a3 172 vec.z = left.x * right.y - left.y * right.x;
Gaku0606 0:deef87aa67a3 173 return vec;
Gaku0606 0:deef87aa67a3 174 }
Gaku0606 0:deef87aa67a3 175
Gaku0606 0:deef87aa67a3 176 /**
Gaku0606 1:8e112552bdfc 177 * @bref 内積を計算します
Gaku0606 1:8e112552bdfc 178 * @note ドットが使えなかったので％になりました。許してヒヤシンス
Gaku0606 0:deef87aa67a3 179 */
Gaku0606 3:3d5d3689521c 180 inline float operator%(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 181 return (left.x * right.x + left.y * right.y + left.z * right.z);
Gaku0606 0:deef87aa67a3 182 }
Gaku0606 0:deef87aa67a3 183
Gaku0606 0:deef87aa67a3 184 /**
Gaku0606 1:8e112552bdfc 185 * @bref Vector3クラスの各要素をスカラー倍します
Gaku0606 0:deef87aa67a3 186 */
Gaku0606 3:3d5d3689521c 187 inline Vector3 operator*(float scalar, Vector3 vec3){
Gaku0606 0:deef87aa67a3 188 static Vector3 vec;
Gaku0606 0:deef87aa67a3 189 vec.x = scalar * vec3.x;
Gaku0606 0:deef87aa67a3 190 vec.y = scalar * vec3.y;
Gaku0606 0:deef87aa67a3 191 vec.z = scalar * vec3.z;
Gaku0606 0:deef87aa67a3 192 return vec;
Gaku0606 0:deef87aa67a3 193 }
Gaku0606 0:deef87aa67a3 194
Gaku0606 0:deef87aa67a3 195 /**
Gaku0606 1:8e112552bdfc 196 * @bref Vector3クラスの各要素をスカラー倍します
Gaku0606 0:deef87aa67a3 197 */
Gaku0606 3:3d5d3689521c 198 inline Vector3 operator*(Vector3 vec3, float scalar){
Gaku0606 0:deef87aa67a3 199 static Vector3 vec;
Gaku0606 0:deef87aa67a3 200 vec.x = scalar * vec3.x;
Gaku0606 0:deef87aa67a3 201 vec.y = scalar * vec3.y;
Gaku0606 0:deef87aa67a3 202 vec.z = scalar * vec3.z;
Gaku0606 0:deef87aa67a3 203 return vec;
Gaku0606 0:deef87aa67a3 204 }
Gaku0606 0:deef87aa67a3 205
Gaku0606 0:deef87aa67a3 206 /**
Gaku0606 1:8e112552bdfc 207 * @bref Vector3クラスの各要素をスカラーで割ります
Gaku0606 0:deef87aa67a3 208 */
Gaku0606 3:3d5d3689521c 209 inline Vector3 operator/(Vector3 vec3, float scalar){
Gaku0606 0:deef87aa67a3 210 static Vector3 vec;
Gaku0606 0:deef87aa67a3 211 vec.x = vec3.x / scalar;
Gaku0606 0:deef87aa67a3 212 vec.y = vec3.y / scalar;
Gaku0606 0:deef87aa67a3 213 vec.z = vec3.z / scalar;
Gaku0606 0:deef87aa67a3 214 return vec;
Gaku0606 0:deef87aa67a3 215 }
Gaku0606 0:deef87aa67a3 216
Gaku0606 3:3d5d3689521c 217 template<typename T>void Vector3::Set(T _x, T _y, T _z){
Gaku0606 0:deef87aa67a3 218 x = _x;
Gaku0606 0:deef87aa67a3 219 y = _y;
Gaku0606 0:deef87aa67a3 220 z = _z;
Gaku0606 0:deef87aa67a3 221 }
Gaku0606 0:deef87aa67a3 222
Gaku0606 3:3d5d3689521c 223 inline float Vector3::Angle(Vector3 vec){
Gaku0606 3:3d5d3689521c 224 float r = (*this % vec) / (this->Norm() * vec.Norm());
Gaku0606 4:d68f06f4d554 225 return acosf(r);
Gaku0606 0:deef87aa67a3 226 }
Gaku0606 0:deef87aa67a3 227
Gaku0606 3:3d5d3689521c 228 inline float Vector3::SgnAngle(Vector3 vec){
Gaku0606 3:3d5d3689521c 229 float theta = this->Angle(vec);
Gaku0606 3:3d5d3689521c 230 vec = *this * vec;
Gaku0606 3:3d5d3689521c 231 if(vec.z < 0) theta *= -1.0;
Gaku0606 3:3d5d3689521c 232
Gaku0606 3:3d5d3689521c 233 return theta;
Gaku0606 3:3d5d3689521c 234 }
Gaku0606 3:3d5d3689521c 235
Gaku0606 3:3d5d3689521c 236 inline float Vector3::Norm(){
Gaku0606 4:d68f06f4d554 237 return sqrtf(x*x + y*y + z*z);
Gaku0606 0:deef87aa67a3 238 }
Gaku0606 0:deef87aa67a3 239
Gaku0606 0:deef87aa67a3 240 #endif