Gaku Matsumoto / Vector3

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

Dependents:   Quaternion HAPS_GPS_Test_0002 GYSFDMAXB HAPS_EKF ... more

Vector3.hpp@0:deef87aa67a3, 2017-06-07 (annotated)

Committer:
Gaku0606
Date:
Wed Jun 07 00:56:18 2017 +0000
Revision:
0:deef87aa67a3
Child:
1:8e112552bdfc
????????????; ??

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 0:deef87aa67a3 27 * @bref ‚RŽŸŒ³ƒxƒNƒgƒ‹‚Ì—v‘f‚ðƒRƒs[
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 0:deef87aa67a3 36 /** ‚RŽŸŒ³ƒxƒNƒgƒ‹‚ð‘«‚µ‚Ä‘ã“ü‚·‚é*/
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 0:deef87aa67a3 44 /** ‚RŽŸŒ³ƒxƒNƒgƒ‹‚ðˆø‚¢‚Ä‘ã“ü‚·‚é*/
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 0:deef87aa67a3 52 /** ‚RŽŸŒ³ƒxƒNƒgƒ‹‚ðŠOÏ‚µ‚Ä‘ã“ü‚·‚é*/
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 0:deef87aa67a3 83 * @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚ð‰Šú‰»‚µ‚Ü‚·B
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 0:deef87aa67a3 88 * @bref ‚Q‚‚ÌVector3ƒNƒ‰ƒX‚Ì‚È‚·Šp‚ðŒvŽZ‚µ‚Ü‚·.
Gaku0606 0:deef87aa67a3 89 * @param Ž©•ª‚Æ‚Ì‚È‚·Šp“x‚ðŒvŽZ‚·‚éVector3ƒNƒ‰ƒX‚̃Cƒ“ƒXƒ^ƒ“ƒX
Gaku0606 0:deef87aa67a3 90 */
Gaku0606 0:deef87aa67a3 91 double Angle(Vector3 v);
Gaku0606 0:deef87aa67a3 92
Gaku0606 0:deef87aa67a3 93 /**
Gaku0606 0:deef87aa67a3 94 * @bref ƒ[ƒƒxƒNƒgƒ‹‚©‚Ç‚¤‚©”»’肵‚Ü‚·.
Gaku0606 0:deef87aa67a3 95 * @note 0œ–@‚ð–hŽ~‚·‚é‚Ì‚ÉŽg‚Á‚Ä‚­‚¾‚³‚¢.
Gaku0606 0:deef87aa67a3 96 * @return 1‚È‚çƒ[ƒƒxƒNƒgƒ‹A0‚È‚çƒ[ƒƒxƒNƒgƒ‹‚Å‚Í‚ ‚è‚Ü‚¹‚ñ.
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 0:deef87aa67a3 106 * @bref Ž©g‚̃mƒ‹ƒ€‚ðŒvŽZ‚µ‚Ä•Ô‚µ‚Ü‚·.
Gaku0606 0:deef87aa67a3 107 */
Gaku0606 0:deef87aa67a3 108 double Norm();
Gaku0606 0:deef87aa67a3 109
Gaku0606 0:deef87aa67a3 110 /**
Gaku0606 0:deef87aa67a3 111 * @bref ’PˆÊƒxƒNƒgƒ‹‚É‚µ‚Ü‚·
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 0:deef87aa67a3 128 * @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚̘a‚ðŒvŽZ‚µ‚Ü‚·B
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 0:deef87aa67a3 139 * @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚̍·‚ðŒvŽZ‚µ‚Ü‚·B
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 0:deef87aa67a3 150 * @bref Vector3ƒNƒ‰ƒX‚ÌŠOÏ‚ðŒvŽZ‚µ‚Ü‚·.
Gaku0606 0:deef87aa67a3 151 * @note ŠOÏ‚Å‚·‚̂ŏ‡˜‚É’ˆÓ‚µ‚Ä‚­‚¾‚³‚¢.
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 0:deef87aa67a3 162 * @bref “àÏ‚ðŒvŽZ‚µ‚Ü‚·
Gaku0606 0:deef87aa67a3 163 * @note ƒhƒbƒg‚ªŽg‚¦‚È‚©‚Á‚½‚̂Ł“‚É‚È‚è‚Ü‚µ‚½B‹–‚µ‚ăqƒ„ƒVƒ“ƒX
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 0:deef87aa67a3 170 * @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚ðƒXƒJƒ‰[”{‚µ‚Ü‚·
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 0:deef87aa67a3 181 * @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚ðƒXƒJƒ‰[”{‚µ‚Ü‚·
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 0:deef87aa67a3 192 * @bref Vector3ƒNƒ‰ƒX‚ÌŠe—v‘f‚ðƒXƒJƒ‰[‚ÅŠ„‚è‚Ü‚·
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