Michael Spencer / Smoothie

Fork of Smoothie to port to mbed non-LPC targets.

Dependencies:   mbed

Fork of Smoothie by Stéphane Cachat

libs/Vector3.cpp@3:f151d08d335c, 2014-03-02 (annotated)

Committer:
Bigcheese
Date:
Sun Mar 02 06:33:08 2014 +0000
Revision:
3:f151d08d335c
Parent:
2:1df0b61d3b5a 
Bunch of stuff. Need to locally merge in updated USB changes.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Michael J. Spencer 2:1df0b61d3b5a 1 #include "Vector3.h"
Michael J. Spencer 2:1df0b61d3b5a 2
Michael J. Spencer 2:1df0b61d3b5a 3 #include <fastmath.h>
Michael J. Spencer 2:1df0b61d3b5a 4 #include <cstddef>
Michael J. Spencer 2:1df0b61d3b5a 5
Michael J. Spencer 2:1df0b61d3b5a 6 float Vector3::nan = NAN;
Michael J. Spencer 2:1df0b61d3b5a 7
Michael J. Spencer 2:1df0b61d3b5a 8 Vector3::Vector3()
Michael J. Spencer 2:1df0b61d3b5a 9 {
Michael J. Spencer 2:1df0b61d3b5a 10 elem[0] = elem[1] = elem[2] = 0.0F;
Michael J. Spencer 2:1df0b61d3b5a 11 }
Michael J. Spencer 2:1df0b61d3b5a 12
Michael J. Spencer 2:1df0b61d3b5a 13 Vector3::Vector3(float a, float b, float c)
Michael J. Spencer 2:1df0b61d3b5a 14 {
Michael J. Spencer 2:1df0b61d3b5a 15 elem[0] = a;
Michael J. Spencer 2:1df0b61d3b5a 16 elem[1] = b;
Michael J. Spencer 2:1df0b61d3b5a 17 elem[2] = c;
Michael J. Spencer 2:1df0b61d3b5a 18 }
Michael J. Spencer 2:1df0b61d3b5a 19
Michael J. Spencer 2:1df0b61d3b5a 20 float& Vector3::operator[](int i)
Michael J. Spencer 2:1df0b61d3b5a 21 {
Michael J. Spencer 2:1df0b61d3b5a 22 if (i >= 0 && i <= 2)
Michael J. Spencer 2:1df0b61d3b5a 23 return elem[i];
Michael J. Spencer 2:1df0b61d3b5a 24 return nan;
Michael J. Spencer 2:1df0b61d3b5a 25 }
Michael J. Spencer 2:1df0b61d3b5a 26
Michael J. Spencer 2:1df0b61d3b5a 27 Vector3 Vector3::cross(const Vector3 vec)
Michael J. Spencer 2:1df0b61d3b5a 28 {
Michael J. Spencer 2:1df0b61d3b5a 29 Vector3 out;
Michael J. Spencer 2:1df0b61d3b5a 30
Michael J. Spencer 2:1df0b61d3b5a 31 out.elem[0] = elem[1] * vec.elem[2] - elem[2] * vec.elem[1];
Michael J. Spencer 2:1df0b61d3b5a 32 out.elem[1] = elem[2] * vec.elem[0] - elem[0] * vec.elem[2];
Michael J. Spencer 2:1df0b61d3b5a 33 out.elem[2] = elem[0] * vec.elem[1] - elem[1] * vec.elem[0];
Michael J. Spencer 2:1df0b61d3b5a 34
Michael J. Spencer 2:1df0b61d3b5a 35 return out;
Michael J. Spencer 2:1df0b61d3b5a 36 }
Michael J. Spencer 2:1df0b61d3b5a 37
Michael J. Spencer 2:1df0b61d3b5a 38 float Vector3::dot(const Vector3 vec)
Michael J. Spencer 2:1df0b61d3b5a 39 {
Michael J. Spencer 2:1df0b61d3b5a 40 return elem[0] * vec.elem[0] +
Michael J. Spencer 2:1df0b61d3b5a 41 elem[1] * vec.elem[1] +
Michael J. Spencer 2:1df0b61d3b5a 42 elem[2] * vec.elem[2];
Michael J. Spencer 2:1df0b61d3b5a 43 }
Michael J. Spencer 2:1df0b61d3b5a 44
Michael J. Spencer 2:1df0b61d3b5a 45 float Vector3::magsq()
Michael J. Spencer 2:1df0b61d3b5a 46 {
Michael J. Spencer 2:1df0b61d3b5a 47 return powf(elem[0], 2) +
Michael J. Spencer 2:1df0b61d3b5a 48 powf(elem[1], 2) +
Michael J. Spencer 2:1df0b61d3b5a 49 powf(elem[2], 2);
Michael J. Spencer 2:1df0b61d3b5a 50 }
Michael J. Spencer 2:1df0b61d3b5a 51
Michael J. Spencer 2:1df0b61d3b5a 52 float Vector3::mag()
Michael J. Spencer 2:1df0b61d3b5a 53 {
Michael J. Spencer 2:1df0b61d3b5a 54 return sqrtf(magsq());
Michael J. Spencer 2:1df0b61d3b5a 55 }
Michael J. Spencer 2:1df0b61d3b5a 56
Michael J. Spencer 2:1df0b61d3b5a 57 Vector3 Vector3::add(const Vector3 vec)
Michael J. Spencer 2:1df0b61d3b5a 58 {
Michael J. Spencer 2:1df0b61d3b5a 59 Vector3 out;
Michael J. Spencer 2:1df0b61d3b5a 60
Michael J. Spencer 2:1df0b61d3b5a 61 out.elem[0] = elem[0] + vec.elem[0];
Michael J. Spencer 2:1df0b61d3b5a 62 out.elem[1] = elem[1] + vec.elem[1];
Michael J. Spencer 2:1df0b61d3b5a 63 out.elem[2] = elem[2] + vec.elem[2];
Michael J. Spencer 2:1df0b61d3b5a 64
Michael J. Spencer 2:1df0b61d3b5a 65 return out;
Michael J. Spencer 2:1df0b61d3b5a 66 }
Michael J. Spencer 2:1df0b61d3b5a 67
Michael J. Spencer 2:1df0b61d3b5a 68 Vector3 Vector3::sub(const Vector3 vec)
Michael J. Spencer 2:1df0b61d3b5a 69 {
Michael J. Spencer 2:1df0b61d3b5a 70 Vector3 out;
Michael J. Spencer 2:1df0b61d3b5a 71
Michael J. Spencer 2:1df0b61d3b5a 72 out.elem[0] = elem[0] - vec.elem[0];
Michael J. Spencer 2:1df0b61d3b5a 73 out.elem[1] = elem[1] - vec.elem[1];
Michael J. Spencer 2:1df0b61d3b5a 74 out.elem[2] = elem[2] - vec.elem[2];
Michael J. Spencer 2:1df0b61d3b5a 75
Michael J. Spencer 2:1df0b61d3b5a 76 return out;
Michael J. Spencer 2:1df0b61d3b5a 77 }
Michael J. Spencer 2:1df0b61d3b5a 78
Michael J. Spencer 2:1df0b61d3b5a 79 Vector3 Vector3::mul(float scalar)
Michael J. Spencer 2:1df0b61d3b5a 80 {
Michael J. Spencer 2:1df0b61d3b5a 81 Vector3 out;
Michael J. Spencer 2:1df0b61d3b5a 82
Michael J. Spencer 2:1df0b61d3b5a 83 out.elem[0] = elem[0] * scalar;
Michael J. Spencer 2:1df0b61d3b5a 84 out.elem[1] = elem[1] * scalar;
Michael J. Spencer 2:1df0b61d3b5a 85 out.elem[2] = elem[2] * scalar;
Michael J. Spencer 2:1df0b61d3b5a 86
Michael J. Spencer 2:1df0b61d3b5a 87 return out;
Michael J. Spencer 2:1df0b61d3b5a 88 }
Michael J. Spencer 2:1df0b61d3b5a 89
Michael J. Spencer 2:1df0b61d3b5a 90 Vector3 Vector3::unit()
Michael J. Spencer 2:1df0b61d3b5a 91 {
Michael J. Spencer 2:1df0b61d3b5a 92 Vector3 out;
Michael J. Spencer 2:1df0b61d3b5a 93
Michael J. Spencer 2:1df0b61d3b5a 94 float denom = mag();
Michael J. Spencer 2:1df0b61d3b5a 95
Michael J. Spencer 2:1df0b61d3b5a 96 out.elem[0] = elem[0] / denom;
Michael J. Spencer 2:1df0b61d3b5a 97 out.elem[1] = elem[1] / denom;
Michael J. Spencer 2:1df0b61d3b5a 98 out.elem[2] = elem[2] / denom;
Michael J. Spencer 2:1df0b61d3b5a 99
Michael J. Spencer 2:1df0b61d3b5a 100 return out;
Michael J. Spencer 2:1df0b61d3b5a 101 }