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@3:c0137be74db4, 2015-03-13 (annotated)
- Committer:
- pvaibhav
- Date:
- Fri Mar 13 09:10:41 2015 +0000
- Revision:
- 3:c0137be74db4
- Parent:
- 2:26ba41c58962
- Child:
- 4:1ced03aa8c75
Added Quaternion constructor from rotation matrix. Fixed Vector3.normalised()
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 | |
p3p | 0:3cc1a808d8c6 | 6 | class Vector3 { |
p3p | 0:3cc1a808d8c6 | 7 | public: |
p3p | 0:3cc1a808d8c6 | 8 | Vector3(){ |
p3p | 0:3cc1a808d8c6 | 9 | x = y = z = 0; |
p3p | 0:3cc1a808d8c6 | 10 | } |
p3p | 0:3cc1a808d8c6 | 11 | |
p3p | 0:3cc1a808d8c6 | 12 | Vector3(float x, float y, float z){ |
p3p | 0:3cc1a808d8c6 | 13 | this->x = x; |
p3p | 0:3cc1a808d8c6 | 14 | this->y = y; |
p3p | 0:3cc1a808d8c6 | 15 | this->z = z; |
p3p | 0:3cc1a808d8c6 | 16 | } |
p3p | 0:3cc1a808d8c6 | 17 | |
p3p | 0:3cc1a808d8c6 | 18 | void set(float x, float y, float z){ |
p3p | 0:3cc1a808d8c6 | 19 | this->x = x; |
p3p | 0:3cc1a808d8c6 | 20 | this->y = y; |
p3p | 0:3cc1a808d8c6 | 21 | this->z = z; |
p3p | 0:3cc1a808d8c6 | 22 | } |
p3p | 0:3cc1a808d8c6 | 23 | |
pvaibhav | 1:857642c51139 | 24 | float length_squared() const{ |
p3p | 0:3cc1a808d8c6 | 25 | return x*x + y*y + z*z; |
p3p | 0:3cc1a808d8c6 | 26 | } |
p3p | 0:3cc1a808d8c6 | 27 | |
pvaibhav | 1:857642c51139 | 28 | float length() const{ |
p3p | 0:3cc1a808d8c6 | 29 | return sqrt(length_squared()); |
p3p | 0:3cc1a808d8c6 | 30 | } |
p3p | 0:3cc1a808d8c6 | 31 | |
p3p | 0:3cc1a808d8c6 | 32 | void normalise(){ |
p3p | 0:3cc1a808d8c6 | 33 | float len = length(); |
p3p | 0:3cc1a808d8c6 | 34 | x = x/len; |
p3p | 0:3cc1a808d8c6 | 35 | y = y/len; |
p3p | 0:3cc1a808d8c6 | 36 | z = z/len; |
p3p | 0:3cc1a808d8c6 | 37 | } |
p3p | 0:3cc1a808d8c6 | 38 | |
pvaibhav | 3:c0137be74db4 | 39 | Vector3 normalised() const{ |
pvaibhav | 3:c0137be74db4 | 40 | return *this / length(); |
pvaibhav | 2:26ba41c58962 | 41 | } |
pvaibhav | 2:26ba41c58962 | 42 | |
pvaibhav | 1:857642c51139 | 43 | float dot_product(const Vector3 &v) const{ |
p3p | 0:3cc1a808d8c6 | 44 | return x*v.x + y*v.y + z*v.z; |
p3p | 0:3cc1a808d8c6 | 45 | } |
p3p | 0:3cc1a808d8c6 | 46 | |
pvaibhav | 1:857642c51139 | 47 | Vector3 cross_product(const Vector3 &v) const{ |
p3p | 0:3cc1a808d8c6 | 48 | Vector3 temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x); |
p3p | 0:3cc1a808d8c6 | 49 | return temp; |
p3p | 0:3cc1a808d8c6 | 50 | } |
p3p | 0:3cc1a808d8c6 | 51 | |
p3p | 0:3cc1a808d8c6 | 52 | //operator overides |
p3p | 0:3cc1a808d8c6 | 53 | void operator ()(const float x, const float y, const float z) { |
p3p | 0:3cc1a808d8c6 | 54 | this->x = x; |
p3p | 0:3cc1a808d8c6 | 55 | this->y = y; |
p3p | 0:3cc1a808d8c6 | 56 | this->z = z; |
p3p | 0:3cc1a808d8c6 | 57 | } |
p3p | 0:3cc1a808d8c6 | 58 | |
pvaibhav | 1:857642c51139 | 59 | bool operator == (const Vector3 &v) const { |
p3p | 0:3cc1a808d8c6 | 60 | return (x==v.x && y==v.y && z==v.z); |
p3p | 0:3cc1a808d8c6 | 61 | } |
p3p | 0:3cc1a808d8c6 | 62 | |
pvaibhav | 1:857642c51139 | 63 | bool operator != (const Vector3 &v) const { |
p3p | 0:3cc1a808d8c6 | 64 | return (x!=v.x || y!=v.y || z!=v.z); |
p3p | 0:3cc1a808d8c6 | 65 | } |
p3p | 0:3cc1a808d8c6 | 66 | |
p3p | 0:3cc1a808d8c6 | 67 | const Vector3 &operator = (const Vector3 &v) { |
p3p | 0:3cc1a808d8c6 | 68 | x = v.x; |
p3p | 0:3cc1a808d8c6 | 69 | y = v.y; |
p3p | 0:3cc1a808d8c6 | 70 | z = v.z; |
p3p | 0:3cc1a808d8c6 | 71 | return *this; |
p3p | 0:3cc1a808d8c6 | 72 | } |
p3p | 0:3cc1a808d8c6 | 73 | |
p3p | 0:3cc1a808d8c6 | 74 | const Vector3 operator - (void) const { |
p3p | 0:3cc1a808d8c6 | 75 | return Vector3(-x,-y,-z); |
p3p | 0:3cc1a808d8c6 | 76 | } |
p3p | 0:3cc1a808d8c6 | 77 | |
p3p | 0:3cc1a808d8c6 | 78 | const Vector3 operator + (const Vector3 &v) const { |
p3p | 0:3cc1a808d8c6 | 79 | return Vector3(x+v.x, y+v.y, z+v.z); |
p3p | 0:3cc1a808d8c6 | 80 | } |
p3p | 0:3cc1a808d8c6 | 81 | |
p3p | 0:3cc1a808d8c6 | 82 | const Vector3 operator - (const Vector3 &v) const { |
p3p | 0:3cc1a808d8c6 | 83 | return Vector3(x-v.x, y-v.y, z-v.z); |
p3p | 0:3cc1a808d8c6 | 84 | } |
p3p | 0:3cc1a808d8c6 | 85 | |
p3p | 0:3cc1a808d8c6 | 86 | const Vector3 operator * (const float num) const { |
p3p | 0:3cc1a808d8c6 | 87 | Vector3 temp; |
p3p | 0:3cc1a808d8c6 | 88 | temp.x = x * num; |
p3p | 0:3cc1a808d8c6 | 89 | temp.y = y * num; |
p3p | 0:3cc1a808d8c6 | 90 | temp.z = z * num; |
p3p | 0:3cc1a808d8c6 | 91 | return temp; |
p3p | 0:3cc1a808d8c6 | 92 | } |
p3p | 0:3cc1a808d8c6 | 93 | |
p3p | 0:3cc1a808d8c6 | 94 | const Vector3 operator / (const float num) const { |
p3p | 0:3cc1a808d8c6 | 95 | Vector3 temp; |
p3p | 0:3cc1a808d8c6 | 96 | temp.x = x / num; |
p3p | 0:3cc1a808d8c6 | 97 | temp.y = y / num; |
p3p | 0:3cc1a808d8c6 | 98 | temp.z = z / num; |
p3p | 0:3cc1a808d8c6 | 99 | return temp; |
p3p | 0:3cc1a808d8c6 | 100 | } |
p3p | 0:3cc1a808d8c6 | 101 | |
p3p | 0:3cc1a808d8c6 | 102 | float operator * (const Vector3 &v) const { |
p3p | 0:3cc1a808d8c6 | 103 | return x*v.x + y*v.y + z*v.z; |
p3p | 0:3cc1a808d8c6 | 104 | } |
p3p | 0:3cc1a808d8c6 | 105 | |
p3p | 0:3cc1a808d8c6 | 106 | float x, y, z; |
p3p | 0:3cc1a808d8c6 | 107 | |
p3p | 0:3cc1a808d8c6 | 108 | }; |
p3p | 0:3cc1a808d8c6 | 109 | |
p3p | 0:3cc1a808d8c6 | 110 | #endif |