minor dead code cleanup
[zumo-cc3200/zumo-cc3200.git] / src / Energia / libraries / ZumoCC3200 / utility / DCM.cpp
1 /*
2  * Copyright (c) 2015, Texas Instruments Incorporated
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * *  Redistributions of source code must retain the above copyright
10  *    notice, this list of conditions and the following disclaimer.
11  *
12  * *  Redistributions in binary form must reproduce the above copyright
13  *    notice, this list of conditions and the following disclaimer in the
14  *    documentation and/or other materials provided with the distribution.
15  *
16  * *  Neither the name of Texas Instruments Incorporated nor the names of
17  *    its contributors may be used to endorse or promote products derived
18  *    from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  */
33 /*
34  * DCM.c
35  *
36  *  Created on: Jul 22, 2015
37  *      Author: x0236197
38  */
39 #include <xdc/std.h>
41 #include <stdlib.h>
43 #include <math.h>
44 #include <stdbool.h>
45 #include <stdint.h>
46 #include <assert.h>
48 #include "DCM.h"
50 typedef struct DCM_Object
51 {
52     /* The state of the direction cosine matrix */
53     float dcm[3][3];
55     /* The time delta between updates to the DCM */
56     float deltaT;
58     /* The scaling factor for DCM update based on the accelerometer reading */
59     float scaleA;
61     /* The scaling factor for DCM update based on the gyroscope reading */
62     float scaleG;
64     /* The scaling factor for DCM update based on the magnetometer reading */
65     float scaleM;
67     /* The most recent accelerometer readings */
68     float pfAccel[3];
70     /* The most recent gyroscope readings */
71     float pfGyro[3];
73     /* The most recent magnetometer readings */
74     float pfMagneto[3];
75 } DCM_Object;
77 static float dotProduct(float v1[3], float v2[3]);
78 static void  crossProduct(float vout[3], float v1[3], float v2[3]);
79 static void  scaleVector(float vout[3], float vin[3], float fScale);
80 static void  addVector(float vout[3], float v1[3], float v2[3]);
81 static void  normalize(float vin[3]);
83 /*
84  *  ======== DCM_create ========
85  */
86 DCM_Handle DCM_create(float deltaT,
87                       float scaleA, float scaleG, float scaleM)
88 {
89     DCM_Handle handle = (DCM_Object *)malloc(sizeof(struct DCM_Object));
91     /* Initialize the DCM matrix to the identity matrix */
92     handle->dcm[0][0] = 1.0f;
93     handle->dcm[0][1] = 0.0f;
94     handle->dcm[0][2] = 0.0f;
95     handle->dcm[1][0] = 0.0f;
96     handle->dcm[1][1] = 1.0f;
97     handle->dcm[1][2] = 0.0f;
98     handle->dcm[2][0] = 0.0f;
99     handle->dcm[2][1] = 0.0f;
100     handle->dcm[2][2] = 1.0f;
102     /* Save the time delta between DCM updates */
103     handle->deltaT = deltaT;
105     /* Save the scaling factors that are applied to the accelerometer,
106        gyroscope, and magnetometer readings */
107     handle->scaleA = scaleA;
108     handle->scaleG = scaleG;
109     handle->scaleM = scaleM;
111     return handle;
114 /*
115  *  ======== DCM_delete ========
116  */
117 void DCM_delete(DCM_Handle handle)
119     free(handle);
122 /*
123  *  ======== DCM_updateAccelData ========
124  *  Updates the accelerometer reading used by the complementary filter DCM
125  *  algorithm.
126  * 
127  *  handle is a pointer to the DCM state structure.
128  *  fAccelX is the accelerometer reading in the X body axis.
129  *  fAccelY is the accelerometer reading in the Y body axis.
130  *  fAccelZ is the accelerometer reading in the Z body axis.
131  * 
132  *  This function updates the accelerometer reading used by the complementary
133  *  filter DCM algorithm.  The accelerometer readings provided to this function
134  *  are used by subsequent calls to DCM_start() and DCM_update() to
135  *  compute the attitude estimate.
136  */
137 void DCM_updateAccelData(DCM_Handle handle,
138                          float fAccelX, float fAccelY, float fAccelZ)
140     /* The user should never pass in values that are not-a-number */
141     assert(!isnan(fAccelX));
142     assert(!isnan(fAccelY));
143     assert(!isnan(fAccelZ));
145     /* Save the new accelerometer reading */
146     handle->pfAccel[0] = fAccelX;
147     handle->pfAccel[1] = fAccelY;
148     handle->pfAccel[2] = fAccelZ;
151 /*
152  *  ======== DCM_updateGyroData ========
153  *  Updates the gyroscope reading used by the complementary filter DCM
154  *  algorithm.
155  * 
156  *  handle is a pointer to the DCM state structure.
157  *  fGyroX is the gyroscope reading in the X body axis.
158  *  fGyroY is the gyroscope reading in the Y body axis.
159  *  fGyroZ is the gyroscope reading in the Z body axis.
160  * 
161  *  This function updates the gyroscope reading used by the complementary
162  *  filter DCM algorithm.  The gyroscope readings provided to this function are
163  *  used by subsequent calls to DCM_update() to compute the attitude
164  *  estimate.
165  */
166 void DCM_updateGyroData(DCM_Handle handle,
167                         float fGyroX, float fGyroY, float fGyroZ)
169     /* The user should never pass in values that are not-a-number */
170     assert(!isnan(fGyroX));
171     assert(!isnan(fGyroY));
172     assert(!isnan(fGyroZ));
174     /* Save the new gyroscope reading */
175     handle->pfGyro[0] = fGyroX;
176     handle->pfGyro[1] = fGyroY;
177     handle->pfGyro[2] = fGyroZ;
180 /*
181  *  ======== DCM_updateMagnetoData ========
182  *  Updates the magnetometer reading used by the complementary filter DCM
183  *  algorithm.
184  * 
185  *  handle is a pointer to the DCM state structure.
186  *  fMagnetoX is the magnetometer reading in the X body axis.
187  *  fMagnetoY is the magnetometer reading in the Y body axis.
188  *  fMagnetoZ is the magnetometer reading in the Z body axis.
189  * 
190  *  This function updates the magnetometer reading used by the complementary
191  *  filter DCM algorithm.  The magnetometer readings provided to this function
192  *  are used by subsequent calls to DCM_start() and DCM_update() to
193  *  compute the attitude estimate.
194  */
195 void DCM_updateMagnetoData(DCM_Handle handle,
196                            float fMagnetoX, float fMagnetoY, float fMagnetoZ)
198     /* The user should never pass in values that are not-a-number */
199     assert(!isnan(fMagnetoX));
200     assert(!isnan(fMagnetoY));
201     assert(!isnan(fMagnetoZ));
203     /* Save the new magnetometer reading */
204     handle->pfMagneto[0] = fMagnetoX;
205     handle->pfMagneto[1] = fMagnetoY;
206     handle->pfMagneto[2] = fMagnetoZ;
209 /*
210  *  ======== DCM_start ========
211  *  Starts the complementary filter DCM attitude estimation from an initial
212  *  sensor reading.
213  * 
214  *  handle is a pointer to the DCM state structure.
215  * 
216  *  This function computes the initial complementary filter DCM attitude
217  *  estimation state based on the initial accelerometer and magnetometer
218  *  reading.  While not necessary for the attitude estimation to converge,
219  *  using an initial state based on sensor readings results in quicker
220  *  convergence.
221  */
222 void DCM_start(DCM_Handle handle)
224     /* The I, J and K basis vectors */
225     float pfI[3], pfJ[3], pfK[3];
227     /* The magnetometer reading forms the initial I vector, pointing north */
228     pfI[0] = handle->pfMagneto[0];
229     pfI[1] = handle->pfMagneto[1];
230     pfI[2] = handle->pfMagneto[2];
232     /* The accelerometer reading forms the initial K vector, pointing down */
233     pfK[0] = handle->pfAccel[0];
234     pfK[1] = handle->pfAccel[1];
235     pfK[2] = handle->pfAccel[2];
237     /* Compute the initial J vector, which is the cross product of the 
238      * K and I vectors
239      */
240     crossProduct(pfJ, pfK, pfI);
242     /* Recompute the I vector from the cross product of the J and K vectors.
243      * This makes it fully orthogonal, which it wasn't before since magnetic
244      * north points inside the Earth in many places. 
245      */ 
246     crossProduct(pfI, pfJ, pfK);
248     /* Normalize the I, J, and K vectors */
249     normalize(pfI);
250     normalize(pfJ);
251     normalize(pfK);
253     /* Initialize the DCM matrix from the I, J, and K vectors */
254     handle->dcm[0][0] = pfI[0];
255     handle->dcm[0][1] = pfI[1];
256     handle->dcm[0][2] = pfI[2];
257     handle->dcm[1][0] = pfJ[0];
258     handle->dcm[1][1] = pfJ[1];
259     handle->dcm[1][2] = pfJ[2];
260     handle->dcm[2][0] = pfK[0];
261     handle->dcm[2][1] = pfK[1];
262     handle->dcm[2][2] = pfK[2];
265 /*
266  *  ======== DCM_update ========
267  *  Updates the complementary filter DCM attitude estimation based on an
268  *  updated set of sensor readings.
269  * 
270  *  handle is a pointer to the DCM state structure.
271  * 
272  *  This function updates the complementary filter DCM attitude estimation
273  *  state based on the current sensor readings.  This function must be called
274  *  at the rate specified in initialization, with new readings supplied at an
275  *  appropriate rate (for example, magnetometers typically sample at a much
276  *  slower rate than accelerometers and gyroscopes).
277  * 
278  *  Notes: split up into smaller operations
279  */
281 void DCM_update(DCM_Handle handle)
283     /* The I, J and K basis vectors */
284     float pfI[3], pfJ[3], pfK[3];
286     /* The rotation from the previous state */
287     float pfDelta[3]; 
288         
289     /* A temporary storage vector */
290     float pfTemp[3];
291         
292     /* The orthogonality error */
293     float fError;
294     bool bNAN = false;
296     /* Form the I, J, K global unit vectors as measured from the body frame
297      * of reference
298      */
300     /* The magnetometer reading forms the new Im vector, pointing north */
301     pfI[0] = handle->pfMagneto[0];
302     pfI[1] = handle->pfMagneto[1];
303     pfI[2] = handle->pfMagneto[2];
305     /* The accelerometer reading forms the new Ka vector, pointing down */
306     pfK[0] = handle->pfAccel[0];
307     pfK[1] = handle->pfAccel[1];
308     pfK[2] = handle->pfAccel[2];
310     /* Compute the new J vector, which is the cross product of the Ka and Im
311      * vectors 
312      */
313     crossProduct(pfJ, pfK, pfI);
315     /* Recompute the Im vector from the cross product of the J and Ka vectors.
316      * This makes it fully orthogonal, which it wasn't before since magnetic
317      * north points inside the Earth in many places 
318      */
319     crossProduct(pfI, pfJ, pfK);
321     /* Normalize the Im and Ka vectors */
322     normalize(pfI);
323     normalize(pfK);
325     /* Compute and scale the rotation as inferred from the accelerometer,
326      * storing it in the rotation accumulator 
327      */
329     /* old K crossed with new K -> axis of K rotation */
330     crossProduct(pfTemp, handle->dcm[2], pfK); 
331     scaleVector(pfDelta, pfTemp, handle->scaleA);
333     /* Compute and scale the rotation as measured by the gyroscope, adding
334      * it to the rotation accumulator 
335      */
336     pfTemp[0] = -(handle->pfGyro[0]) * (handle->deltaT) * (handle->scaleG); 
337     pfTemp[1] = -(handle->pfGyro[1]) * (handle->deltaT) * (handle->scaleG); 
338     pfTemp[2] = -(handle->pfGyro[2]) * (handle->deltaT) * (handle->scaleG); 
339     addVector(pfDelta, pfDelta, pfTemp);
341     /* Compute and scale the rotation as inferred from the magnetometer, 
342      * adding it to the rotation accumulator.
343      * 
344      * Note: old I crossed with new I -> axis of I rotation
345      */
346     crossProduct(pfTemp, handle->dcm[0], pfI);
347     scaleVector(pfTemp, pfTemp, handle->scaleM);
348     addVector(pfDelta, pfDelta, pfTemp);
350     /* Rotate the I vector from the DCM matrix by the scaled rotation
351      *     rotation vector crossed with old I
352      *     old I += new I
353      */
354     crossProduct(pfI, pfDelta, handle->dcm[0]); 
355     addVector(handle->dcm[0], handle->dcm[0], pfI); 
357     /* Rotate the K vector from the DCM matrix by the scaled rotation
358      *     rotation vector crossed with old K
359      *     old K += new K
360      */
361     crossProduct(pfK, pfDelta, handle->dcm[2]); 
362     addVector(handle->dcm[2], handle->dcm[2], pfK); 
364     /* Compute the orthogonality error between the rotated I and K vectors and
365      * adjust each by half the error, bringing them closer to orthogonality
366      * cross couple to cancel out excess rotation:
367      *     I = I + err*K
368      *     K = K + err*I
369      */
370     fError = dotProduct(handle->dcm[0], handle->dcm[2]) / -2.0f;
371     scaleVector(pfI, handle->dcm[0], fError);
372     scaleVector(pfK, handle->dcm[2], fError);
373     addVector(handle->dcm[0], handle->dcm[0], pfK);  
374     addVector(handle->dcm[2], handle->dcm[2], pfI);
376     /* Normalize the I and K vectors.
377      *     Use the Taylor expansion around 1 to simplify calculations
378      */
379     scaleVector(handle->dcm[0], handle->dcm[0],
380                 0.5f * (3.0f - dotProduct(handle->dcm[0], handle->dcm[0])));
381     scaleVector(handle->dcm[2], handle->dcm[2],
382                 0.5f * (3.0f - dotProduct(handle->dcm[2], handle->dcm[2])));
384     /* Compute the rotated J vector from the cross product of the rotated,
385      * corrected K and I vectors 
386      */
387     crossProduct(handle->dcm[1], handle->dcm[2], handle->dcm[0]);
389     /* Determine if the newly updated DCM contains any invalid (i.e.,
390      * NaN) values 
391      */
392     bNAN = (isnan(handle->dcm[0][0]) || isnan(handle->dcm[0][1])
393             || isnan(handle->dcm[0][2]) || isnan(handle->dcm[1][0])
394             || isnan(handle->dcm[1][1]) || isnan(handle->dcm[1][2])
395             || isnan(handle->dcm[2][0]) || isnan(handle->dcm[2][1])
396             || isnan(handle->dcm[2][2]));
398     /* As a debug measure, we check for NaN in the DCM.  The user can trap
399      * this event depending on their implementation of __error__.  Should they
400      * choose to disable interrupts and loop forever then they will have
401      * preserved the stack and can analyze how they arrived at NaN 
402      */
403     assert(!bNAN);
405     /* If any part of the matrix is not-a-number then reset the DCM back to 
406      * the identity matrix 
407      */
408     if (bNAN) {
409         handle->dcm[0][0] = 1.0f;
410         handle->dcm[0][1] = 0.0f;
411         handle->dcm[0][2] = 0.0f;
412         handle->dcm[1][0] = 0.0f;
413         handle->dcm[1][1] = 1.0f;
414         handle->dcm[1][2] = 0.0f;
415         handle->dcm[2][0] = 0.0f;
416         handle->dcm[2][1] = 0.0f;
417         handle->dcm[2][2] = 1.0f;
418     }
421 /*
422  *  ======== DCM_getMatrix ========
423  *  Assigns the input matrix with the current DCM values.
424  * 
425  *  handle is a pointer to the DCM state structure.
426  *  dcm is a pointer to the array into which to store the DCM matrix
427  *  values.
428  */
429 void DCM_getMatrix(DCM_Handle handle, float dcm[3][3])
431     /* set the input matrix as the DCM */
432     dcm[0][0] = handle->dcm[0][0];
433     dcm[0][1] = handle->dcm[0][1];
434     dcm[0][2] = handle->dcm[0][2];
435     dcm[1][0] = handle->dcm[1][0];
436     dcm[1][1] = handle->dcm[1][1];
437     dcm[1][2] = handle->dcm[1][2];
438     dcm[2][0] = handle->dcm[2][0];
439     dcm[2][1] = handle->dcm[2][1];
440     dcm[2][2] = handle->dcm[2][2];
443 /*
444  *  ======== DCM_computeEulers ========
445  *  Computes the Euler angles from the DCM attitude estimation matrix.
446  * 
447  *  handle is a pointer to the DCM state structure.
448  *  pfRoll is a pointer to the value into which the roll is stored.
449  *  pfPitch is a pointer to the value into which the pitch is stored.
450  *  pfYaw is a pointer to the value into which the yaw is stored.
451  * 
452  *  This function computes the Euler angles that are represented by the DCM
453  *  attitude estimation matrix.  If any of the Euler angles is not required,
454  *  the corresponding parameter can be \b NULL.
455  */
456 void DCM_computeEulers(DCM_Handle handle,
457                        float *pfRoll, float *pfPitch,  float *pfYaw)
459     /* Compute the roll, pitch, and yaw as required */
460     if (pfRoll) {
461         *pfRoll = atan2f(handle->dcm[2][1], handle->dcm[2][2]);
462     }
463     if (pfPitch) {
464         *pfPitch = -asinf(handle->dcm[2][0]);
465     }
466     if (pfYaw) {
467         *pfYaw = atan2f(handle->dcm[1][0], handle->dcm[0][0]);
468     }
471 /*
472  *  ======== DCM_computeQuaternion ========
473  *  Computes the quaternion from the DCM attitude estimation matrix.
474  * 
475  *  handle is a pointer to the DCM state structure.
476  *  pfQuaternion is an array into which the quaternion is stored.
477  * 
478  *  This function computes the quaternion that is represented by the DCM
479  *  attitude estimation matrix.
480  */
481 void DCM_computeQuaternion(DCM_Handle handle, float pfQuaternion[4])
483     float fQs, fQx, fQy, fQz;
485     /* Partially compute Qs, Qx, Qy, and Qz based on the DCM diagonals.  The
486      * square root, an expensive operation, is computed for only one of these
487      * as determined later 
488      */
489     fQs = 1 + handle->dcm[0][0] + handle->dcm[1][1] + handle->dcm[2][2];
490     fQx = 1 + handle->dcm[0][0] - handle->dcm[1][1] - handle->dcm[2][2];
491     fQy = 1 - handle->dcm[0][0] + handle->dcm[1][1] - handle->dcm[2][2];
492     fQz = 1 - handle->dcm[0][0] - handle->dcm[1][1] + handle->dcm[2][2];
494     /* See if Qs is the largest of the diagonal values */
495     if ((fQs > fQx) && (fQs > fQy) && (fQs > fQz)) {
496         /* Finish the computation of Qs */
497         fQs = sqrtf(fQs) / 2;
499         /* Compute the values of the quaternion based on Qs */
500         pfQuaternion[0] = fQs;
501         pfQuaternion[1] = ((handle->dcm[2][1] - handle->dcm[1][2]) /
502                            (4 * fQs));
503         pfQuaternion[2] = ((handle->dcm[0][2] - handle->dcm[2][0]) /
504                            (4 * fQs));
505         pfQuaternion[3] = ((handle->dcm[1][0] - handle->dcm[0][1]) /
506                            (4 * fQs));
507     }
508     /* Qs is not the largest, so see if Qx is the largest remaining diagonal
509      * value 
510      */
511     else if ((fQx > fQy) && (fQx > fQz)) {
512         /* Finish the computation of Qx */
513         fQx = sqrtf(fQx) / 2;
515         /* Compute the values of the quaternion based on Qx */
516         pfQuaternion[0] = ((handle->dcm[2][1] - handle->dcm[1][2]) /
517                            (4 * fQx));
518         pfQuaternion[1] = fQx;
519         pfQuaternion[2] = ((handle->dcm[1][0] + handle->dcm[0][1]) /
520                            (4 * fQx));
521         pfQuaternion[3] = ((handle->dcm[0][2] + handle->dcm[2][0]) /
522                            (4 * fQx));
523     }
524     /* Qs and Qx are not the largest, so see if Qy is the largest remaining
525      * diagonal value 
526      */
527     else if (fQy > fQz)
528     {
529         /* Finish the computation of Qy */
530         fQy = sqrtf(fQy) / 2;
532         /* Compute the values of the quaternion based on Qy */
533         pfQuaternion[0] = ((handle->dcm[0][2] - handle->dcm[2][0]) /
534                            (4 * fQy));
535         pfQuaternion[1] = ((handle->dcm[1][0] + handle->dcm[0][1]) /
536                            (4 * fQy));
537         pfQuaternion[2] = fQy;
538         pfQuaternion[3] = ((handle->dcm[2][1] + handle->dcm[1][2]) /
539                            (4 * fQy));
540     }
541     /* Qz is the largest diagonal value */
542     else {
543         /* Finish the computation of Qz */
544         fQz = sqrtf(fQz) / 2;
546         /* Compute the values of the quaternion based on Qz */
547         pfQuaternion[0] = ((handle->dcm[1][0] - handle->dcm[0][1]) /
548                            (4 * fQz));
549         pfQuaternion[1] = ((handle->dcm[0][2] + handle->dcm[2][0]) /
550                            (4 * fQz));
551         pfQuaternion[2] = ((handle->dcm[2][1] + handle->dcm[1][2]) /
552                            (4 * fQz));
553         pfQuaternion[3] = fQz;
554     }
557 /*
558  *  ======== dotProduct ========
559  *  Computes the dot product of two vectors.
560  * 
561  *  v1 is the first vector.
562  *  v2 is the second vector.
563  * 
564  *  This function computes the dot product of two 3-dimensional vector.
565  * 
566  *  Returns the dot product of the two vectors.
567  */
568 static float dotProduct(float v1[3], float v2[3])
570     return ((v1[0] * v2[0]) + (v1[1] * v2[1]) + (v1[2] * v2[2]));
573 /*
574  *  ======== crossProduct ========
575  *  Computes the cross product of two vectors.
576  * 
577  *  vout is the output vector.
578  *  v1 is the first vector.
579  *  v2 is the second vector.
580  * 
581  *  This function computes the cross product of two 3-dimensional vectors.
582  */
583 static void crossProduct(float vout[3], float v1[3], float v2[3])
585     vout[0] = ((v1[1] * v2[2]) - (v1[2] * v2[1]));
586     vout[1] = ((v1[2] * v2[0]) - (v1[0] * v2[2]));
587     vout[2] = ((v1[0] * v2[1]) - (v1[1] * v2[0]));
590 /*
591  *  ======== scaleVector ========
592  *  Scales a vector.
593  * 
594  *  vout is the output vector.
595  *  vin is the input vector.
596  *  fScale is the scale factor.
597  * 
598  *  This function scales a 3-dimensional vector by multiplying each of its
599  *  components by the scale factor.
600  */
601 static void scaleVector(float vout[3], float vin[3], float fScale)
603     vout[0] = vin[0] * fScale;
604     vout[1] = vin[1] * fScale;
605     vout[2] = vin[2] * fScale;
608 /*
609  *  ======== addVector ========
610  *  Adds two vectors.
611  * 
612  *  vout is the output vector.
613  *  v1 is the first vector.
614  *  v2 is the second vector.
615  * 
616  *  This function adds two 3-dimensional vectors.
617  */
618 static void addVector(float vout[3], float v1[3], float v2[3])
620     vout[0] = v1[0] + v2[0];
621     vout[1] = v1[1] + v2[1];
622     vout[2] = v1[2] + v2[2];
625 /*
626  *  ======== normalize ========
627  *  Normalizes a vector
628  * 
629  *  vin is the input vector.
630  * 
631  *  This function normalizes a 3-dimensional vectors by dividing its
632  *  components by its magnitude.
633  */
634 static void normalize(float vin[3])
636     float magnitude = sqrtf(dotProduct(vin, vin));
637     vin[0] = (vin[0] / magnitude);
638     vin[1] = (vin[1] / magnitude);
639     vin[2] = (vin[2] / magnitude);