A quick implementation of Quaternion and Vector classes for use with my MPU9150 library
Dependents: cool_step_new cool_step_1 SML2
Fork of QuaternionMath by
Vector3.h@10:69f240dbf0df, 2015-05-27 (annotated)
- Committer:
- pvaibhav
- Date:
- Wed May 27 13:01:34 2015 +0000
- Revision:
- 10:69f240dbf0df
- Parent:
- 8:08d9c0010cc0
Code reformatted
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
p3p | 0:3cc1a808d8c6 | 1 | #ifndef __AHRSMATHDSP_VECTOR3_ |
p3p | 0:3cc1a808d8c6 | 2 | #define __AHRSMATHDSP_VECTOR3_ |
p3p | 0:3cc1a808d8c6 | 3 | |
pvaibhav | 1:857642c51139 | 4 | //#include "arm_math.h" |
p3p | 0:3cc1a808d8c6 | 5 | |
pvaibhav | 4:1ced03aa8c75 | 6 | class Vector3 |
pvaibhav | 4:1ced03aa8c75 | 7 | { |
p3p | 0:3cc1a808d8c6 | 8 | public: |
pvaibhav | 8:08d9c0010cc0 | 9 | Vector3() : x(0.0f), y(0.0f), z(0.0f) {} |
pvaibhav | 8:08d9c0010cc0 | 10 | |
pvaibhav | 8:08d9c0010cc0 | 11 | Vector3(float const _x, float const _y, float const _z) |
pvaibhav | 8:08d9c0010cc0 | 12 | : x(_x), y(_y), z(_z) {} |
pvaibhav | 4:1ced03aa8c75 | 13 | |
pvaibhav | 8:08d9c0010cc0 | 14 | void set(float const _x, float const _y, float const _z) { |
pvaibhav | 8:08d9c0010cc0 | 15 | x = _x; |
pvaibhav | 8:08d9c0010cc0 | 16 | y = _y; |
pvaibhav | 8:08d9c0010cc0 | 17 | z = _z; |
p3p | 0:3cc1a808d8c6 | 18 | } |
pvaibhav | 4:1ced03aa8c75 | 19 | |
pvaibhav | 4:1ced03aa8c75 | 20 | float length_squared() const { |
pvaibhav | 8:08d9c0010cc0 | 21 | return x * x + y * y + z * z; |
p3p | 0:3cc1a808d8c6 | 22 | } |
pvaibhav | 4:1ced03aa8c75 | 23 | |
pvaibhav | 4:1ced03aa8c75 | 24 | float length() const { |
pvaibhav | 4:1ced03aa8c75 | 25 | return sqrt(length_squared()); |
p3p | 0:3cc1a808d8c6 | 26 | } |
pvaibhav | 4:1ced03aa8c75 | 27 | |
pvaibhav | 4:1ced03aa8c75 | 28 | void normalise() { |
pvaibhav | 8:08d9c0010cc0 | 29 | float const len = length(); |
pvaibhav | 8:08d9c0010cc0 | 30 | x = x / len; |
pvaibhav | 8:08d9c0010cc0 | 31 | y = y / len; |
pvaibhav | 8:08d9c0010cc0 | 32 | z = z / len; |
p3p | 0:3cc1a808d8c6 | 33 | } |
pvaibhav | 4:1ced03aa8c75 | 34 | |
pvaibhav | 4:1ced03aa8c75 | 35 | Vector3 normalised() const { |
pvaibhav | 3:c0137be74db4 | 36 | return *this / length(); |
pvaibhav | 2:26ba41c58962 | 37 | } |
pvaibhav | 4:1ced03aa8c75 | 38 | |
pvaibhav | 4:1ced03aa8c75 | 39 | float dot_product(const Vector3 &v) const { |
pvaibhav | 8:08d9c0010cc0 | 40 | return x * v.x + y * v.y + z * v.z; |
p3p | 0:3cc1a808d8c6 | 41 | } |
pvaibhav | 4:1ced03aa8c75 | 42 | |
pvaibhav | 4:1ced03aa8c75 | 43 | Vector3 cross_product(const Vector3 &v) const { |
pvaibhav | 8:08d9c0010cc0 | 44 | Vector3 temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); |
p3p | 0:3cc1a808d8c6 | 45 | return temp; |
p3p | 0:3cc1a808d8c6 | 46 | } |
pvaibhav | 4:1ced03aa8c75 | 47 | |
pvaibhav | 8:08d9c0010cc0 | 48 | // operator overides |
pvaibhav | 8:08d9c0010cc0 | 49 | void operator()(const float _x, const float _y, const float _z) { |
pvaibhav | 8:08d9c0010cc0 | 50 | x = _x; |
pvaibhav | 8:08d9c0010cc0 | 51 | y = _y; |
pvaibhav | 8:08d9c0010cc0 | 52 | z = _z; |
p3p | 0:3cc1a808d8c6 | 53 | } |
p3p | 0:3cc1a808d8c6 | 54 | |
pvaibhav | 8:08d9c0010cc0 | 55 | bool operator==(const Vector3 &v) const { |
pvaibhav | 8:08d9c0010cc0 | 56 | return (x == v.x && y == v.y && z == v.z); |
p3p | 0:3cc1a808d8c6 | 57 | } |
p3p | 0:3cc1a808d8c6 | 58 | |
pvaibhav | 8:08d9c0010cc0 | 59 | bool operator!=(const Vector3 &v) const { |
pvaibhav | 8:08d9c0010cc0 | 60 | return (x != v.x || y != v.y || z != v.z); |
p3p | 0:3cc1a808d8c6 | 61 | } |
p3p | 0:3cc1a808d8c6 | 62 | |
pvaibhav | 8:08d9c0010cc0 | 63 | const Vector3 &operator=(const Vector3 &v) { |
p3p | 0:3cc1a808d8c6 | 64 | x = v.x; |
p3p | 0:3cc1a808d8c6 | 65 | y = v.y; |
pvaibhav | 4:1ced03aa8c75 | 66 | z = v.z; |
p3p | 0:3cc1a808d8c6 | 67 | return *this; |
p3p | 0:3cc1a808d8c6 | 68 | } |
pvaibhav | 4:1ced03aa8c75 | 69 | |
pvaibhav | 8:08d9c0010cc0 | 70 | const Vector3 operator-(void) const { |
pvaibhav | 8:08d9c0010cc0 | 71 | return Vector3(-x, -y, -z); |
p3p | 0:3cc1a808d8c6 | 72 | } |
pvaibhav | 4:1ced03aa8c75 | 73 | |
pvaibhav | 8:08d9c0010cc0 | 74 | const Vector3 operator+(const Vector3 &v) const { |
pvaibhav | 8:08d9c0010cc0 | 75 | return Vector3(x + v.x, y + v.y, z + v.z); |
p3p | 0:3cc1a808d8c6 | 76 | } |
pvaibhav | 4:1ced03aa8c75 | 77 | |
pvaibhav | 8:08d9c0010cc0 | 78 | const Vector3 operator-(const Vector3 &v) const { |
pvaibhav | 8:08d9c0010cc0 | 79 | return Vector3(x - v.x, y - v.y, z - v.z); |
p3p | 0:3cc1a808d8c6 | 80 | } |
p3p | 0:3cc1a808d8c6 | 81 | |
pvaibhav | 8:08d9c0010cc0 | 82 | const Vector3 operator*(const float num) const { |
pvaibhav | 8:08d9c0010cc0 | 83 | return Vector3(x * num, y * num, z * num); |
p3p | 0:3cc1a808d8c6 | 84 | } |
pvaibhav | 4:1ced03aa8c75 | 85 | |
pvaibhav | 8:08d9c0010cc0 | 86 | const Vector3 operator/(const float num) const { |
pvaibhav | 8:08d9c0010cc0 | 87 | return Vector3(x / num, y / num, z / num); |
pvaibhav | 8:08d9c0010cc0 | 88 | } |
pvaibhav | 8:08d9c0010cc0 | 89 | |
pvaibhav | 8:08d9c0010cc0 | 90 | float operator*(const Vector3 &v) const { |
pvaibhav | 8:08d9c0010cc0 | 91 | return x * v.x + y * v.y + z * v.z; |
p3p | 0:3cc1a808d8c6 | 92 | } |
pvaibhav | 4:1ced03aa8c75 | 93 | |
p3p | 0:3cc1a808d8c6 | 94 | float x, y, z; |
p3p | 0:3cc1a808d8c6 | 95 | }; |
p3p | 0:3cc1a808d8c6 | 96 | |
p3p | 0:3cc1a808d8c6 | 97 | #endif |