Gaku Matsumoto / Vector3

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

Dependents:   Quaternion HAPS_GPS_Test_0002 GYSFDMAXB HAPS_EKF ... more

Vector3.hpp@3:3d5d3689521c, 2017-12-16 (annotated)

Committer:
Gaku0606
Date:
Sat Dec 16 02:38:25 2017 +0000
Revision:
3:3d5d3689521c
Parent:
1:8e112552bdfc
Child:
4:d68f06f4d554
float????

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 3:3d5d3689521c 121 float norm = sqrt(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 3:3d5d3689521c 133
Gaku0606 0:deef87aa67a3 134 };
Gaku0606 0:deef87aa67a3 135
Gaku0606 0:deef87aa67a3 136 /**
Gaku0606 1:8e112552bdfc 137 * @bref Vector3クラスの各要素の和を計算します。
Gaku0606 0:deef87aa67a3 138 */
Gaku0606 0:deef87aa67a3 139 inline Vector3 operator+(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 140 static Vector3 vec;
Gaku0606 0:deef87aa67a3 141 vec.x = left.x + right.x;
Gaku0606 0:deef87aa67a3 142 vec.y = left.y + right.y;
Gaku0606 0:deef87aa67a3 143 vec.z = left.z + right.z;
Gaku0606 0:deef87aa67a3 144 return vec;
Gaku0606 0:deef87aa67a3 145 }
Gaku0606 0:deef87aa67a3 146
Gaku0606 0:deef87aa67a3 147 /**
Gaku0606 1:8e112552bdfc 148 * @bref Vector3クラスの各要素の差を計算します。
Gaku0606 0:deef87aa67a3 149 */
Gaku0606 0:deef87aa67a3 150 inline Vector3 operator-(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 151 static Vector3 vec;
Gaku0606 0:deef87aa67a3 152 vec.x = left.x - right.x;
Gaku0606 0:deef87aa67a3 153 vec.y = left.y - right.y;
Gaku0606 0:deef87aa67a3 154 vec.z = left.z - right.z;
Gaku0606 0:deef87aa67a3 155 return vec;
Gaku0606 0:deef87aa67a3 156 }
Gaku0606 0:deef87aa67a3 157
Gaku0606 0:deef87aa67a3 158 /**
Gaku0606 1:8e112552bdfc 159 * @bref Vector3クラスの外積を計算します.
Gaku0606 1:8e112552bdfc 160 * @note 外積ですので順序に注意してください.
Gaku0606 0:deef87aa67a3 161 */
Gaku0606 0:deef87aa67a3 162 inline Vector3 operator*(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 163 static Vector3 vec;
Gaku0606 0:deef87aa67a3 164 vec.x = left.y * right.z - left.z * right.y;
Gaku0606 0:deef87aa67a3 165 vec.y = left.z * right.x - left.x * right.z;
Gaku0606 0:deef87aa67a3 166 vec.z = left.x * right.y - left.y * right.x;
Gaku0606 0:deef87aa67a3 167 return vec;
Gaku0606 0:deef87aa67a3 168 }
Gaku0606 0:deef87aa67a3 169
Gaku0606 0:deef87aa67a3 170 /**
Gaku0606 1:8e112552bdfc 171 * @bref 内積を計算します
Gaku0606 1:8e112552bdfc 172 * @note ドットが使えなかったので％になりました。許してヒヤシンス
Gaku0606 0:deef87aa67a3 173 */
Gaku0606 3:3d5d3689521c 174 inline float operator%(Vector3 left, Vector3 right){
Gaku0606 0:deef87aa67a3 175 return (left.x * right.x + left.y * right.y + left.z * right.z);
Gaku0606 0:deef87aa67a3 176 }
Gaku0606 0:deef87aa67a3 177
Gaku0606 0:deef87aa67a3 178 /**
Gaku0606 1:8e112552bdfc 179 * @bref Vector3クラスの各要素をスカラー倍します
Gaku0606 0:deef87aa67a3 180 */
Gaku0606 3:3d5d3689521c 181 inline Vector3 operator*(float scalar, Vector3 vec3){
Gaku0606 0:deef87aa67a3 182 static Vector3 vec;
Gaku0606 0:deef87aa67a3 183 vec.x = scalar * vec3.x;
Gaku0606 0:deef87aa67a3 184 vec.y = scalar * vec3.y;
Gaku0606 0:deef87aa67a3 185 vec.z = scalar * vec3.z;
Gaku0606 0:deef87aa67a3 186 return vec;
Gaku0606 0:deef87aa67a3 187 }
Gaku0606 0:deef87aa67a3 188
Gaku0606 0:deef87aa67a3 189 /**
Gaku0606 1:8e112552bdfc 190 * @bref Vector3クラスの各要素をスカラー倍します
Gaku0606 0:deef87aa67a3 191 */
Gaku0606 3:3d5d3689521c 192 inline Vector3 operator*(Vector3 vec3, float scalar){
Gaku0606 0:deef87aa67a3 193 static Vector3 vec;
Gaku0606 0:deef87aa67a3 194 vec.x = scalar * vec3.x;
Gaku0606 0:deef87aa67a3 195 vec.y = scalar * vec3.y;
Gaku0606 0:deef87aa67a3 196 vec.z = scalar * vec3.z;
Gaku0606 0:deef87aa67a3 197 return vec;
Gaku0606 0:deef87aa67a3 198 }
Gaku0606 0:deef87aa67a3 199
Gaku0606 0:deef87aa67a3 200 /**
Gaku0606 1:8e112552bdfc 201 * @bref Vector3クラスの各要素をスカラーで割ります
Gaku0606 0:deef87aa67a3 202 */
Gaku0606 3:3d5d3689521c 203 inline Vector3 operator/(Vector3 vec3, float scalar){
Gaku0606 0:deef87aa67a3 204 static Vector3 vec;
Gaku0606 0:deef87aa67a3 205 vec.x = vec3.x / scalar;
Gaku0606 0:deef87aa67a3 206 vec.y = vec3.y / scalar;
Gaku0606 0:deef87aa67a3 207 vec.z = vec3.z / scalar;
Gaku0606 0:deef87aa67a3 208 return vec;
Gaku0606 0:deef87aa67a3 209 }
Gaku0606 0:deef87aa67a3 210
Gaku0606 3:3d5d3689521c 211 template<typename T>void Vector3::Set(T _x, T _y, T _z){
Gaku0606 0:deef87aa67a3 212 x = _x;
Gaku0606 0:deef87aa67a3 213 y = _y;
Gaku0606 0:deef87aa67a3 214 z = _z;
Gaku0606 0:deef87aa67a3 215 }
Gaku0606 0:deef87aa67a3 216
Gaku0606 3:3d5d3689521c 217 inline float Vector3::Angle(Vector3 vec){
Gaku0606 3:3d5d3689521c 218 float r = (*this % vec) / (this->Norm() * vec.Norm());
Gaku0606 0:deef87aa67a3 219 return acos(r);
Gaku0606 0:deef87aa67a3 220 }
Gaku0606 0:deef87aa67a3 221
Gaku0606 3:3d5d3689521c 222 inline float Vector3::SgnAngle(Vector3 vec){
Gaku0606 3:3d5d3689521c 223 float theta = this->Angle(vec);
Gaku0606 3:3d5d3689521c 224 vec = *this * vec;
Gaku0606 3:3d5d3689521c 225 if(vec.z < 0) theta *= -1.0;
Gaku0606 3:3d5d3689521c 226
Gaku0606 3:3d5d3689521c 227 return theta;
Gaku0606 3:3d5d3689521c 228 }
Gaku0606 3:3d5d3689521c 229
Gaku0606 3:3d5d3689521c 230 inline float Vector3::Norm(){
Gaku0606 0:deef87aa67a3 231 return sqrt(x*x + y*y + z*z);
Gaku0606 0:deef87aa67a3 232 }
Gaku0606 0:deef87aa67a3 233
Gaku0606 0:deef87aa67a3 234 #endif