Yelfie / LocalPositionSystem

Dependents:   TDP_main_BartFork TDP_main TDP_main TDP_main_fork

LPS.cpp@3:dd68ac680416, 2015-03-12 (annotated)

Committer:
Joseph_Penikis
Date:
Thu Mar 12 22:12:18 2015 +0000
Revision:
3:dd68ac680416
Parent:
2:21ca29888540 
Implemented ISP control, not tested...;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joseph_Penikis 1:b71a1c39d263 1 #include "LPS.h"
Joseph_Penikis 0:dda98f02d71e 2
Joseph_Penikis 0:dda98f02d71e 3 // Defines the delay experienced between transmitting IR and Ultrasonic pulse
Joseph_Penikis 0:dda98f02d71e 4 #define PING_OFFSET 0
Joseph_Penikis 3:dd68ac680416 5 #define SPEED_OF_SOUND 330.f
Joseph_Penikis 0:dda98f02d71e 6 // Used to rectify the issue of measured spheres not intersecting, may not be needed, but available anyway
Joseph_Penikis 0:dda98f02d71e 7 #define SWELL_VALUE 0
Joseph_Penikis 0:dda98f02d71e 8
Joseph_Penikis 3:dd68ac680416 9 // Defines for instructions on SPI link, instructions corrpsond to distances under ~20mm, way too close to ever be picked up...
Joseph_Penikis 3:dd68ac680416 10 #define START_CAL 0xFF
Joseph_Penikis 3:dd68ac680416 11 #define REQUEST_T1 0xFE
Joseph_Penikis 3:dd68ac680416 12 #define REQUEST_T2 0xFD
Joseph_Penikis 3:dd68ac680416 13 #define REQUEST_T3 0xFC
Joseph_Penikis 3:dd68ac680416 14 #define NEXT_CAL_POINT 0xFB
Joseph_Penikis 3:dd68ac680416 15 #define UPPER_BYTE 0x01
Joseph_Penikis 3:dd68ac680416 16
Joseph_Penikis 3:dd68ac680416 17 LPS::LPS(PinName MOSI, PinName MISO, PinName SCLK) : _spi(MOSI, MISO, SCLK) {
Joseph_Penikis 3:dd68ac680416 18 // Set up SPI interface for AVR
Joseph_Penikis 3:dd68ac680416 19 _spi.format(8,3);
Joseph_Penikis 3:dd68ac680416 20 _spi.frequency(1000000);
Joseph_Penikis 3:dd68ac680416 21 }
Joseph_Penikis 1:b71a1c39d263 22 LPS::~LPS() {}
Joseph_Penikis 0:dda98f02d71e 23
Joseph_Penikis 1:b71a1c39d263 24 _3D_Vector LPS::getUnitX() { return unitX; }
Joseph_Penikis 1:b71a1c39d263 25 _3D_Vector LPS::getUnitY() { return unitY; }
Joseph_Penikis 1:b71a1c39d263 26 _3D_Vector LPS::getUnitZ() { return unitZ; }
Joseph_Penikis 1:b71a1c39d263 27 float LPS::getd() { return d; }
Joseph_Penikis 1:b71a1c39d263 28 float LPS::geti() { return i; }
Joseph_Penikis 1:b71a1c39d263 29 float LPS::getj() { return j; }
Joseph_Penikis 0:dda98f02d71e 30
Joseph_Penikis 1:b71a1c39d263 31 void LPS::updateLocation() {
Joseph_Penikis 0:dda98f02d71e 32 updateDistances();
Joseph_Penikis 0:dda98f02d71e 33
Joseph_Penikis 0:dda98f02d71e 34 current_1.x = calcX(beacon_1_distance, beacon_2_distance, d);
Joseph_Penikis 0:dda98f02d71e 35 current_1.y = calcY(beacon_1_distance, beacon_3_distance, i, j, current_1.x);
Joseph_Penikis 0:dda98f02d71e 36 current_1.z = calcZ(beacon_1_distance, current_1.x, current_1.y);
Joseph_Penikis 0:dda98f02d71e 37
Joseph_Penikis 0:dda98f02d71e 38 _3D_Vector scaledX = scaleVector(unitX, current_1.x);
Joseph_Penikis 0:dda98f02d71e 39 _3D_Vector scaledY = scaleVector(unitY, current_1.y);
Joseph_Penikis 0:dda98f02d71e 40 _3D_Vector scaledZ = scaleVector(unitZ, current_1.z);
Joseph_Penikis 0:dda98f02d71e 41
Joseph_Penikis 0:dda98f02d71e 42 current_1.x = beacon_1_loc.x + scaledX.x + scaledY.x;
Joseph_Penikis 0:dda98f02d71e 43 current_1.y = beacon_1_loc.y + scaledX.y + scaledY.y;
Joseph_Penikis 0:dda98f02d71e 44 current_1.z = beacon_1_loc.z + scaledX.z + scaledY.z;
Joseph_Penikis 0:dda98f02d71e 45
Joseph_Penikis 0:dda98f02d71e 46 current_2.x = current_1.x - scaledZ.x;
Joseph_Penikis 0:dda98f02d71e 47 current_2.y = current_1.y - scaledZ.y;
Joseph_Penikis 0:dda98f02d71e 48 current_2.z = current_1.z - scaledZ.z;
Joseph_Penikis 0:dda98f02d71e 49
Joseph_Penikis 0:dda98f02d71e 50 current_1.x += scaledZ.x;
Joseph_Penikis 0:dda98f02d71e 51 current_1.y += scaledZ.y;
Joseph_Penikis 0:dda98f02d71e 52 current_1.z += scaledZ.z;
Joseph_Penikis 0:dda98f02d71e 53
Joseph_Penikis 0:dda98f02d71e 54 // Set location closest to z=0 as location, (We are typically on the floor, but need to verify this solution works)
Joseph_Penikis 0:dda98f02d71e 55 // If current_2.z is smaller than current_1.z, swap them
Joseph_Penikis 0:dda98f02d71e 56 if (abs(current_1.z) > abs(current_2.z)) {
Joseph_Penikis 0:dda98f02d71e 57 _3D_Vector temp = current_1;
Joseph_Penikis 0:dda98f02d71e 58 current_1.x = current_2.x;
Joseph_Penikis 0:dda98f02d71e 59 current_1.y = current_2.y;
Joseph_Penikis 0:dda98f02d71e 60 current_1.z = current_2.z;
Joseph_Penikis 0:dda98f02d71e 61
Joseph_Penikis 0:dda98f02d71e 62 current_2.x = temp.x;
Joseph_Penikis 0:dda98f02d71e 63 current_2.y = temp.y;
Joseph_Penikis 0:dda98f02d71e 64 current_2.z = temp.z;
Joseph_Penikis 0:dda98f02d71e 65 }
Joseph_Penikis 0:dda98f02d71e 66
Joseph_Penikis 0:dda98f02d71e 67 // Both solutions are currently preserved incase of neccisity in future use
Joseph_Penikis 0:dda98f02d71e 68 }
Joseph_Penikis 0:dda98f02d71e 69
Joseph_Penikis 1:b71a1c39d263 70 void LPS::calibratePosition(float iCal, float dCal, float jCal) {
Joseph_Penikis 0:dda98f02d71e 71 updateCalibrationDistances();
Joseph_Penikis 0:dda98f02d71e 72
Joseph_Penikis 0:dda98f02d71e 73 // Calculate the coordinates of beacon 1
Joseph_Penikis 0:dda98f02d71e 74 beacon_1_loc.x = calcX(beacon_2_loc.x, beacon_3_loc.x, dCal);
Joseph_Penikis 0:dda98f02d71e 75 beacon_1_loc.y = calcY(beacon_2_loc.x, beacon_1_distance, iCal, jCal, beacon_1_loc.x);
Joseph_Penikis 0:dda98f02d71e 76 beacon_1_loc.z = calcZ(beacon_2_loc.x, beacon_1_loc.x, beacon_1_loc.y);
Joseph_Penikis 0:dda98f02d71e 77
Joseph_Penikis 0:dda98f02d71e 78 // Push the data to new locations while becon 2 is calculated
Joseph_Penikis 0:dda98f02d71e 79 beacon_3_loc.x = beacon_2_loc.y; // 2_t1
Joseph_Penikis 0:dda98f02d71e 80 beacon_1_distance = beacon_2_loc.z; // 3_t1
Joseph_Penikis 0:dda98f02d71e 81
Joseph_Penikis 0:dda98f02d71e 82 beacon_2_loc.x = calcX(beacon_3_loc.x, beacon_3_loc.y, dCal);
Joseph_Penikis 0:dda98f02d71e 83 beacon_2_loc.y = calcY(beacon_3_loc.x, beacon_2_distance, iCal, jCal, beacon_2_loc.x);
Joseph_Penikis 0:dda98f02d71e 84 beacon_2_loc.z = calcZ(beacon_3_loc.x, beacon_2_loc.x, beacon_2_loc.y);
Joseph_Penikis 0:dda98f02d71e 85
Joseph_Penikis 0:dda98f02d71e 86 // Again, push data stored in beacon_3_loc out to safe location
Joseph_Penikis 0:dda98f02d71e 87 beacon_2_distance = beacon_3_loc.z; // 3_t2
Joseph_Penikis 0:dda98f02d71e 88
Joseph_Penikis 0:dda98f02d71e 89 beacon_3_loc.x = calcX(beacon_1_distance, beacon_2_distance, dCal);
Joseph_Penikis 0:dda98f02d71e 90 beacon_3_loc.y = calcY(beacon_1_distance, beacon_3_distance, iCal, jCal, beacon_3_loc.x);
Joseph_Penikis 0:dda98f02d71e 91 beacon_3_loc.z = calcZ(beacon_1_distance, beacon_3_loc.x, beacon_3_loc.y);
Joseph_Penikis 0:dda98f02d71e 92
Joseph_Penikis 0:dda98f02d71e 93 // All beacon locations should now have been acquired (Untested)
Joseph_Penikis 0:dda98f02d71e 94
Joseph_Penikis 0:dda98f02d71e 95 // Is it possible to calculate air speed and calibrate? TO VERIFY
Joseph_Penikis 0:dda98f02d71e 96
Joseph_Penikis 0:dda98f02d71e 97 // Now need to calculate unit vectors and translation values for i, j, and d
Joseph_Penikis 0:dda98f02d71e 98 // This is only required upon power on, and/or if beacons are moved
Joseph_Penikis 0:dda98f02d71e 99 calcUnitVectorsAndScalars();
Joseph_Penikis 0:dda98f02d71e 100 // Calibration complete!!!!!!!
Joseph_Penikis 0:dda98f02d71e 101 }
Joseph_Penikis 0:dda98f02d71e 102
Joseph_Penikis 3:dd68ac680416 103 int LPS::fetchTimeOverSPI(int instr) {
Joseph_Penikis 3:dd68ac680416 104
Joseph_Penikis 3:dd68ac680416 105 // Loop while avr has not prepared data, i.e. still returning an instruction not data. TODO: Make a fetch only if available option to save on processor time
Joseph_Penikis 3:dd68ac680416 106 do {
Joseph_Penikis 3:dd68ac680416 107 received_2 = _spi.write(instr + UPPER_BYTE);
Joseph_Penikis 3:dd68ac680416 108 } while (received_2 & 0x80);
Joseph_Penikis 0:dda98f02d71e 109
Joseph_Penikis 3:dd68ac680416 110 // I have received some data back, the avr is setup to create an identical packet to check data is correct
Joseph_Penikis 3:dd68ac680416 111 // Send this data back to AVR (Not really used here, but better than using defined instructions)
Joseph_Penikis 3:dd68ac680416 112 do {
Joseph_Penikis 3:dd68ac680416 113 // Each time this loop data_1 shifted to r_1, data_2 loaded into r_2, i.e. r_2 ALWAYS has the most recent value
Joseph_Penikis 3:dd68ac680416 114 received_1 = received_2;
Joseph_Penikis 3:dd68ac680416 115 received_2 = _spi.write(received_1);
Joseph_Penikis 3:dd68ac680416 116 } while (received_1 != received_2);
Joseph_Penikis 3:dd68ac680416 117 // Valid data got (UPPER BYTE)
Joseph_Penikis 0:dda98f02d71e 118
Joseph_Penikis 3:dd68ac680416 119 do {
Joseph_Penikis 3:dd68ac680416 120 received_3 = _spi.write(instr);
Joseph_Penikis 3:dd68ac680416 121 } while (received_3 == instr);
Joseph_Penikis 3:dd68ac680416 122 do {
Joseph_Penikis 3:dd68ac680416 123 // Each time this loop data_1 shifted to r_1, data_2 loaded into r_3, i.e. r_3 ALWAYS has the most recent value
Joseph_Penikis 3:dd68ac680416 124 received_1 = received_3;
Joseph_Penikis 3:dd68ac680416 125 received_3 = _spi.write(received_1);
Joseph_Penikis 3:dd68ac680416 126 } while (received_1 != received_3);
Joseph_Penikis 3:dd68ac680416 127 // Valid data got (LOWER BYTE)
Joseph_Penikis 3:dd68ac680416 128
Joseph_Penikis 3:dd68ac680416 129 // Generate an integer from these and return
Joseph_Penikis 3:dd68ac680416 130 return (received_2 << 8) + received_3;
Joseph_Penikis 0:dda98f02d71e 131 }
Joseph_Penikis 0:dda98f02d71e 132
Joseph_Penikis 1:b71a1c39d263 133 void LPS::updateDistances() {
Joseph_Penikis 3:dd68ac680416 134 // This will be the "socket" for talking to the base station
Joseph_Penikis 3:dd68ac680416 135 /*
Joseph_Penikis 3:dd68ac680416 136 beacon_1_distance = (fetchTimeOverSPI(REQUEST_T1) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
Joseph_Penikis 3:dd68ac680416 137 beacon_2_distance = (fetchTimeOverSPI(REQUEST_T2) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
Joseph_Penikis 3:dd68ac680416 138 beacon_3_distance = (fetchTimeOverSPI(REQUEST_T3) - PING_OFFSET) * SPEED_OF_SOUND + SWELL_VALUE;
Joseph_Penikis 3:dd68ac680416 139 */
Joseph_Penikis 3:dd68ac680416 140
Joseph_Penikis 3:dd68ac680416 141 // Just dummy values for testing purposes
Joseph_Penikis 3:dd68ac680416 142 beacon_1_distance = 11.55f;
Joseph_Penikis 3:dd68ac680416 143 beacon_2_distance = 21.095f;
Joseph_Penikis 3:dd68ac680416 144 beacon_3_distance = 15.395f;
Joseph_Penikis 0:dda98f02d71e 145 }
Joseph_Penikis 0:dda98f02d71e 146
Joseph_Penikis 1:b71a1c39d263 147 void LPS::updateCalibrationDistances() {
Joseph_Penikis 3:dd68ac680416 148 // Tell base station to enter calibration mode
Joseph_Penikis 3:dd68ac680416 149 _spi.write(START_CAL);
Joseph_Penikis 3:dd68ac680416 150
Joseph_Penikis 0:dda98f02d71e 151 // Reuse existing function to constrain external accessor code
Joseph_Penikis 3:dd68ac680416 152 updateDistances();
Joseph_Penikis 0:dda98f02d71e 153
Joseph_Penikis 0:dda98f02d71e 154 beacon_2_loc.x = 14.14213562f;//beacon_1_distance; // 1_t1
Joseph_Penikis 0:dda98f02d71e 155 beacon_2_loc.y = 22.49444376f;//beacon_2_distance; // 1_t2
Joseph_Penikis 0:dda98f02d71e 156 beacon_2_loc.z = 12.36931688f;//beacon_3_distance; // 1_t3
Joseph_Penikis 0:dda98f02d71e 157
Joseph_Penikis 3:dd68ac680416 158 updateDistances();
Joseph_Penikis 0:dda98f02d71e 159
Joseph_Penikis 0:dda98f02d71e 160 beacon_3_loc.x = 14.45683229f;//beacon_1_distance; // 2_t1
Joseph_Penikis 0:dda98f02d71e 161 beacon_3_loc.y = 21.47091055f;//beacon_2_distance; // 2_t2
Joseph_Penikis 0:dda98f02d71e 162 beacon_3_loc.z = 12.24744871f;//beacon_3_distance; // 2_t3
Joseph_Penikis 0:dda98f02d71e 163
Joseph_Penikis 3:dd68ac680416 164 updateDistances();
Joseph_Penikis 0:dda98f02d71e 165
Joseph_Penikis 0:dda98f02d71e 166 beacon_1_distance = 16.673332f;
Joseph_Penikis 0:dda98f02d71e 167 beacon_2_distance = 22.36067477f;
Joseph_Penikis 0:dda98f02d71e 168 beacon_3_distance = 10.81665383f;
Joseph_Penikis 0:dda98f02d71e 169 // Third set is stored in the original defined variables, freeing up beacon_1_loc for initial calculation
Joseph_Penikis 0:dda98f02d71e 170 }
Joseph_Penikis 0:dda98f02d71e 171
Joseph_Penikis 2:21ca29888540 172 float calcX(float t1, float t2, float d) {
Joseph_Penikis 0:dda98f02d71e 173 /*
Joseph_Penikis 0:dda98f02d71e 174 x = (t1^2 - t2^2 + d^2) / 2d
Joseph_Penikis 0:dda98f02d71e 175 */
Joseph_Penikis 0:dda98f02d71e 176 return (t1 * t1 - t2 * t2 + d * d) / (2 * d);
Joseph_Penikis 0:dda98f02d71e 177 }
Joseph_Penikis 0:dda98f02d71e 178
Joseph_Penikis 2:21ca29888540 179 float calcY(float t1, float t3, float i, float j, float x) {
Joseph_Penikis 0:dda98f02d71e 180 /*
Joseph_Penikis 0:dda98f02d71e 181 y = (t1^2 - t3^2 +i^2 + j^2) / 2j - (i/j) * x
Joseph_Penikis 0:dda98f02d71e 182 */
Joseph_Penikis 0:dda98f02d71e 183 return ((pow(t1, 2) - pow(t3, 2) + pow(i, 2) + pow(j, 2)) / (2 * j)) - (i / j) * x;
Joseph_Penikis 0:dda98f02d71e 184 }
Joseph_Penikis 0:dda98f02d71e 185
Joseph_Penikis 2:21ca29888540 186 float calcZ(float t1, float x, float y) {
Joseph_Penikis 0:dda98f02d71e 187 /*
Joseph_Penikis 0:dda98f02d71e 188 z = sqrt(t1^2 - x^2 - y^2)
Joseph_Penikis 0:dda98f02d71e 189 */
Joseph_Penikis 0:dda98f02d71e 190 // Technically has two solutions both +/-, can I assume always positive? TODO: Handle inverse value if needed
Joseph_Penikis 0:dda98f02d71e 191 return sqrt(pow(t1, 2) - pow(x, 2) - pow(y, 2));
Joseph_Penikis 0:dda98f02d71e 192 }
Joseph_Penikis 0:dda98f02d71e 193
Joseph_Penikis 1:b71a1c39d263 194 void LPS::calcUnitVectorsAndScalars() {
Joseph_Penikis 0:dda98f02d71e 195 // e_x = P2 - P1 / |P2 - P1|
Joseph_Penikis 0:dda98f02d71e 196 _3D_Vector v = subTwoVectors(beacon_2_loc, beacon_1_loc);
Joseph_Penikis 0:dda98f02d71e 197
Joseph_Penikis 0:dda98f02d71e 198 unitX = unitVector(v);
Joseph_Penikis 0:dda98f02d71e 199
Joseph_Penikis 0:dda98f02d71e 200 // i = e_x dot P3 - P1
Joseph_Penikis 0:dda98f02d71e 201 v = subTwoVectors(beacon_3_loc, beacon_1_loc);
Joseph_Penikis 0:dda98f02d71e 202
Joseph_Penikis 0:dda98f02d71e 203 i = dot_Product(unitX, v);
Joseph_Penikis 0:dda98f02d71e 204
Joseph_Penikis 0:dda98f02d71e 205 // e_y = P3 - P1 - i.e_x / |P3 - P1 - i.e_x|
Joseph_Penikis 0:dda98f02d71e 206 v = subTwoVectors(v, scaleVector(unitX, i));
Joseph_Penikis 0:dda98f02d71e 207
Joseph_Penikis 0:dda98f02d71e 208 unitY = unitVector(v);
Joseph_Penikis 0:dda98f02d71e 209
Joseph_Penikis 0:dda98f02d71e 210 // e_z = e_x cross e_y
Joseph_Penikis 0:dda98f02d71e 211 unitZ = cross_Product(unitX, unitY);
Joseph_Penikis 0:dda98f02d71e 212
Joseph_Penikis 0:dda98f02d71e 213 // d = |P2 - P1|
Joseph_Penikis 0:dda98f02d71e 214 v = subTwoVectors(beacon_2_loc, beacon_1_loc);
Joseph_Penikis 0:dda98f02d71e 215
Joseph_Penikis 0:dda98f02d71e 216 d = vectorMagnitude(v);
Joseph_Penikis 0:dda98f02d71e 217
Joseph_Penikis 0:dda98f02d71e 218 // j = e_y dot P3 - P1
Joseph_Penikis 0:dda98f02d71e 219 v = subTwoVectors(beacon_3_loc, beacon_1_loc);
Joseph_Penikis 0:dda98f02d71e 220
Joseph_Penikis 0:dda98f02d71e 221 j = dot_Product(unitY, v);
Joseph_Penikis 0:dda98f02d71e 222 }
Joseph_Penikis 0:dda98f02d71e 223
Joseph_Penikis 1:b71a1c39d263 224 _3D_Vector LPS::getCurrentLocation() { return current_1; }
Joseph_Penikis 1:b71a1c39d263 225 _3D_Vector LPS::getBeacon_1_Location() { return beacon_1_loc; }
Joseph_Penikis 1:b71a1c39d263 226 _3D_Vector LPS::getBeacon_2_Location() { return beacon_2_loc; }
Joseph_Penikis 1:b71a1c39d263 227 _3D_Vector LPS::getBeacon_3_Location() { return beacon_3_loc; }
Joseph_Penikis 0:dda98f02d71e 228
Joseph_Penikis 0:dda98f02d71e 229 _3D_Vector addFourVectors(_3D_Vector a, _3D_Vector b, _3D_Vector c, _3D_Vector d) {
Joseph_Penikis 0:dda98f02d71e 230 _3D_Vector v;
Joseph_Penikis 0:dda98f02d71e 231
Joseph_Penikis 0:dda98f02d71e 232 v.x = a.x + b.x + c.x + d.x;
Joseph_Penikis 0:dda98f02d71e 233 v.y = a.y + b.y + c.y + d.y;
Joseph_Penikis 0:dda98f02d71e 234 v.z = a.z + b.z + c.z + d.z;
Joseph_Penikis 0:dda98f02d71e 235
Joseph_Penikis 0:dda98f02d71e 236 return v;
Joseph_Penikis 0:dda98f02d71e 237 }
Joseph_Penikis 0:dda98f02d71e 238
Joseph_Penikis 0:dda98f02d71e 239 _3D_Vector scaleVector(_3D_Vector a, float scale) {
Joseph_Penikis 0:dda98f02d71e 240 a.x *= scale;
Joseph_Penikis 0:dda98f02d71e 241 a.y *= scale;
Joseph_Penikis 0:dda98f02d71e 242 a.z *= scale;
Joseph_Penikis 0:dda98f02d71e 243
Joseph_Penikis 0:dda98f02d71e 244 return a;
Joseph_Penikis 0:dda98f02d71e 245 }
Joseph_Penikis 0:dda98f02d71e 246
Joseph_Penikis 0:dda98f02d71e 247 _3D_Vector unitVector(_3D_Vector a) {
Joseph_Penikis 0:dda98f02d71e 248 _3D_Vector v;
Joseph_Penikis 0:dda98f02d71e 249 float size = sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
Joseph_Penikis 0:dda98f02d71e 250
Joseph_Penikis 0:dda98f02d71e 251 v.x = a.x / size;
Joseph_Penikis 0:dda98f02d71e 252 v.y = a.y / size;
Joseph_Penikis 0:dda98f02d71e 253 v.z = a.z / size;
Joseph_Penikis 0:dda98f02d71e 254
Joseph_Penikis 0:dda98f02d71e 255 return v;
Joseph_Penikis 0:dda98f02d71e 256 }
Joseph_Penikis 0:dda98f02d71e 257
Joseph_Penikis 0:dda98f02d71e 258 float dot_Product(_3D_Vector a, _3D_Vector b) {
Joseph_Penikis 0:dda98f02d71e 259 return a.x * b.x + a.y * b.y + a.z * b.z;
Joseph_Penikis 0:dda98f02d71e 260 }
Joseph_Penikis 0:dda98f02d71e 261
Joseph_Penikis 0:dda98f02d71e 262 float vectorMagnitude(_3D_Vector a) {
Joseph_Penikis 0:dda98f02d71e 263 return sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
Joseph_Penikis 0:dda98f02d71e 264 }
Joseph_Penikis 0:dda98f02d71e 265
Joseph_Penikis 0:dda98f02d71e 266 _3D_Vector cross_Product(_3D_Vector a, _3D_Vector b) {
Joseph_Penikis 0:dda98f02d71e 267 _3D_Vector v;
Joseph_Penikis 0:dda98f02d71e 268
Joseph_Penikis 0:dda98f02d71e 269 v.x = (a.y * b.z - a.z * b.y);
Joseph_Penikis 0:dda98f02d71e 270 v.y = (a.z * b.x - a.x * b.z);
Joseph_Penikis 0:dda98f02d71e 271 v.z = (a.x * b.y - a.y * b.x);
Joseph_Penikis 0:dda98f02d71e 272
Joseph_Penikis 0:dda98f02d71e 273 return v;
Joseph_Penikis 0:dda98f02d71e 274 }
Joseph_Penikis 0:dda98f02d71e 275
Joseph_Penikis 0:dda98f02d71e 276 _3D_Vector subTwoVectors(_3D_Vector a, _3D_Vector b) {
Joseph_Penikis 0:dda98f02d71e 277 _3D_Vector v;
Joseph_Penikis 0:dda98f02d71e 278
Joseph_Penikis 0:dda98f02d71e 279 v.x = a.x - b.x;
Joseph_Penikis 0:dda98f02d71e 280 v.y = a.y - b.y;
Joseph_Penikis 0:dda98f02d71e 281 v.z = a.z - b.z;
Joseph_Penikis 0:dda98f02d71e 282
Joseph_Penikis 0:dda98f02d71e 283 return v;
Joseph_Penikis 0:dda98f02d71e 284 }