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 Chris Pepper

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?

UserRevisionLine numberNew 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