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@0:3cc1a808d8c6, 2014-09-01 (annotated)
- Committer:
- p3p
- Date:
- Mon Sep 01 13:48:26 2014 +0000
- Revision:
- 0:3cc1a808d8c6
- Child:
- 1:857642c51139
quick Quaternion and Vector classes
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 | |
p3p | 0:3cc1a808d8c6 | 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 | |
p3p | 0:3cc1a808d8c6 | 24 | float length_squared(){ |
p3p | 0:3cc1a808d8c6 | 25 | return x*x + y*y + z*z; |
p3p | 0:3cc1a808d8c6 | 26 | } |
p3p | 0:3cc1a808d8c6 | 27 | |
p3p | 0:3cc1a808d8c6 | 28 | float length(){ |
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 | |
p3p | 0:3cc1a808d8c6 | 39 | float dot_product(const Vector3 &v){ |
p3p | 0:3cc1a808d8c6 | 40 | return x*v.x + y*v.y + z*v.z; |
p3p | 0:3cc1a808d8c6 | 41 | } |
p3p | 0:3cc1a808d8c6 | 42 | |
p3p | 0:3cc1a808d8c6 | 43 | Vector3 cross_product(const Vector3 &v){ |
p3p | 0:3cc1a808d8c6 | 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 | } |
p3p | 0:3cc1a808d8c6 | 47 | |
p3p | 0:3cc1a808d8c6 | 48 | //operator overides |
p3p | 0:3cc1a808d8c6 | 49 | void operator ()(const float x, const float y, const float z) { |
p3p | 0:3cc1a808d8c6 | 50 | this->x = x; |
p3p | 0:3cc1a808d8c6 | 51 | this->y = y; |
p3p | 0:3cc1a808d8c6 | 52 | this->z = z; |
p3p | 0:3cc1a808d8c6 | 53 | } |
p3p | 0:3cc1a808d8c6 | 54 | |
p3p | 0:3cc1a808d8c6 | 55 | bool operator == (const Vector3 &v) { |
p3p | 0:3cc1a808d8c6 | 56 | return (x==v.x && y==v.y && z==v.z); |
p3p | 0:3cc1a808d8c6 | 57 | } |
p3p | 0:3cc1a808d8c6 | 58 | |
p3p | 0:3cc1a808d8c6 | 59 | bool operator != (const Vector3 &v) { |
p3p | 0:3cc1a808d8c6 | 60 | return (x!=v.x || y!=v.y || z!=v.z); |
p3p | 0:3cc1a808d8c6 | 61 | } |
p3p | 0:3cc1a808d8c6 | 62 | |
p3p | 0:3cc1a808d8c6 | 63 | const Vector3 &operator = (const Vector3 &v) { |
p3p | 0:3cc1a808d8c6 | 64 | x = v.x; |
p3p | 0:3cc1a808d8c6 | 65 | y = v.y; |
p3p | 0:3cc1a808d8c6 | 66 | z = v.z; |
p3p | 0:3cc1a808d8c6 | 67 | return *this; |
p3p | 0:3cc1a808d8c6 | 68 | } |
p3p | 0:3cc1a808d8c6 | 69 | |
p3p | 0:3cc1a808d8c6 | 70 | const Vector3 operator - (void) const { |
p3p | 0:3cc1a808d8c6 | 71 | return Vector3(-x,-y,-z); |
p3p | 0:3cc1a808d8c6 | 72 | } |
p3p | 0:3cc1a808d8c6 | 73 | |
p3p | 0:3cc1a808d8c6 | 74 | const Vector3 operator + (const Vector3 &v) const { |
p3p | 0:3cc1a808d8c6 | 75 | return Vector3(x+v.x, y+v.y, z+v.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 float num) const { |
p3p | 0:3cc1a808d8c6 | 83 | Vector3 temp; |
p3p | 0:3cc1a808d8c6 | 84 | temp.x = x * num; |
p3p | 0:3cc1a808d8c6 | 85 | temp.y = y * num; |
p3p | 0:3cc1a808d8c6 | 86 | temp.z = z * num; |
p3p | 0:3cc1a808d8c6 | 87 | return temp; |
p3p | 0:3cc1a808d8c6 | 88 | } |
p3p | 0:3cc1a808d8c6 | 89 | |
p3p | 0:3cc1a808d8c6 | 90 | const Vector3 operator / (const float num) const { |
p3p | 0:3cc1a808d8c6 | 91 | Vector3 temp; |
p3p | 0:3cc1a808d8c6 | 92 | temp.x = x / num; |
p3p | 0:3cc1a808d8c6 | 93 | temp.y = y / num; |
p3p | 0:3cc1a808d8c6 | 94 | temp.z = z / num; |
p3p | 0:3cc1a808d8c6 | 95 | return temp; |
p3p | 0:3cc1a808d8c6 | 96 | } |
p3p | 0:3cc1a808d8c6 | 97 | |
p3p | 0:3cc1a808d8c6 | 98 | float operator * (const Vector3 &v) const { |
p3p | 0:3cc1a808d8c6 | 99 | return x*v.x + y*v.y + z*v.z; |
p3p | 0:3cc1a808d8c6 | 100 | } |
p3p | 0:3cc1a808d8c6 | 101 | |
p3p | 0:3cc1a808d8c6 | 102 | float x, y, z; |
p3p | 0:3cc1a808d8c6 | 103 | |
p3p | 0:3cc1a808d8c6 | 104 | }; |
p3p | 0:3cc1a808d8c6 | 105 | |
p3p | 0:3cc1a808d8c6 | 106 | #endif |