90f1fbe3b0cb17748f5846535009acd38cce0890
[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  *  ======== cancelDrift ========
211  *  Use orientation reference vectors to cancel out gyro drift with a PI
212  *  feedback controller. This method assumes that the device experiences 
213  *  negligible acceleration. Adjustments can be made to allow this algorithm
214  *  to work with accelerating systems. Only called internally within the 
215  *  DCM calculuation.
216  */
217 static void cancelDrift(DCM_Handle handle)
219     /* two correction vectors fed into the controller */
220     float pfKError[3], pfIError[3]; 
221         
222     /* calcluate the Z-axis correction based on accelerometer reading */
223     crossProduct(pfKError, handle->dcm[2], handle->pfAccel); 
224         
225     /* calcluate the X-axis correction based on magnetometer reading */
226     crossProduct(pfIError, handle->dcm[0], handle->pfMagneto); 
229 /*
230  *  ======== DCM_start ========
231  *  Starts the complementary filter DCM attitude estimation from an initial
232  *  sensor reading.
233  * 
234  *  handle is a pointer to the DCM state structure.
235  * 
236  *  This function computes the initial complementary filter DCM attitude
237  *  estimation state based on the initial accelerometer and magnetometer
238  *  reading.  While not necessary for the attitude estimation to converge,
239  *  using an initial state based on sensor readings results in quicker
240  *  convergence.
241  */
242 void DCM_start(DCM_Handle handle)
244     /* The I, J and K basis vectors */
245     float pfI[3], pfJ[3], pfK[3];
247     /* The magnetometer reading forms the initial I vector, pointing north */
248     pfI[0] = handle->pfMagneto[0];
249     pfI[1] = handle->pfMagneto[1];
250     pfI[2] = handle->pfMagneto[2];
252     /* The accelerometer reading forms the initial K vector, pointing down */
253     pfK[0] = handle->pfAccel[0];
254     pfK[1] = handle->pfAccel[1];
255     pfK[2] = handle->pfAccel[2];
257     /* Compute the initial J vector, which is the cross product of the 
258      * K and I vectors
259      */
260     crossProduct(pfJ, pfK, pfI);
262     /* Recompute the I vector from the cross product of the J and K vectors.
263      * This makes it fully orthogonal, which it wasn't before since magnetic
264      * north points inside the Earth in many places. 
265      */ 
266     crossProduct(pfI, pfJ, pfK);
268     /* Normalize the I, J, and K vectors */
269     normalize(pfI);
270     normalize(pfJ);
271     normalize(pfK);
273     /* Initialize the DCM matrix from the I, J, and K vectors */
274     handle->dcm[0][0] = pfI[0];
275     handle->dcm[0][1] = pfI[1];
276     handle->dcm[0][2] = pfI[2];
277     handle->dcm[1][0] = pfJ[0];
278     handle->dcm[1][1] = pfJ[1];
279     handle->dcm[1][2] = pfJ[2];
280     handle->dcm[2][0] = pfK[0];
281     handle->dcm[2][1] = pfK[1];
282     handle->dcm[2][2] = pfK[2];
285 /*
286  *  ======== DCM_update ========
287  *  Updates the complementary filter DCM attitude estimation based on an
288  *  updated set of sensor readings.
289  * 
290  *  handle is a pointer to the DCM state structure.
291  * 
292  *  This function updates the complementary filter DCM attitude estimation
293  *  state based on the current sensor readings.  This function must be called
294  *  at the rate specified in initialization, with new readings supplied at an
295  *  appropriate rate (for example, magnetometers typically sample at a much
296  *  slower rate than accelerometers and gyroscopes).
297  * 
298  *  Notes: split up into smaller operations
299  */
301 void DCM_update(DCM_Handle handle)
303     /* The I, J and K basis vectors */
304     float pfI[3], pfJ[3], pfK[3];
306     /* The rotation from the previous state */
307     float pfDelta[3]; 
308         
309     /* A temporary storage vector */
310     float pfTemp[3];
311         
312     /* The orthogonality error */
313     float fError;
314     bool bNAN = false;
316     /* Form the I, J, K global unit vectors as measured from the body frame
317      * of reference
318      */
320     /* The magnetometer reading forms the new Im vector, pointing north */
321     pfI[0] = handle->pfMagneto[0];
322     pfI[1] = handle->pfMagneto[1];
323     pfI[2] = handle->pfMagneto[2];
325     /* The accelerometer reading forms the new Ka vector, pointing down */
326     pfK[0] = handle->pfAccel[0];
327     pfK[1] = handle->pfAccel[1];
328     pfK[2] = handle->pfAccel[2];
330     /* Compute the new J vector, which is the cross product of the Ka and Im
331      * vectors 
332      */
333     crossProduct(pfJ, pfK, pfI);
335     /* Recompute the Im vector from the cross product of the J and Ka vectors.
336      * This makes it fully orthogonal, which it wasn't before since magnetic
337      * north points inside the Earth in many places 
338      */
339     crossProduct(pfI, pfJ, pfK);
341     /* Normalize the Im and Ka vectors */
342     normalize(pfI);
343     normalize(pfK);
345     /* Compute and scale the rotation as inferred from the accelerometer,
346      * storing it in the rotation accumulator 
347      */
349     /* old K crossed with new K -> axis of K rotation */
350     crossProduct(pfTemp, handle->dcm[2], pfK); 
351     scaleVector(pfDelta, pfTemp, handle->scaleA);
353     /* Compute and scale the rotation as measured by the gyroscope, adding
354      * it to the rotation accumulator 
355      */
356     pfTemp[0] = -(handle->pfGyro[0]) * (handle->deltaT) * (handle->scaleG); 
357     pfTemp[1] = -(handle->pfGyro[1]) * (handle->deltaT) * (handle->scaleG); 
358     pfTemp[2] = -(handle->pfGyro[2]) * (handle->deltaT) * (handle->scaleG); 
359     addVector(pfDelta, pfDelta, pfTemp);
361     /* Compute and scale the rotation as inferred from the magnetometer, 
362      * adding it to the rotation accumulator.
363      * 
364      * Note: old I crossed with new I -> axis of I rotation
365      */
366     crossProduct(pfTemp, handle->dcm[0], pfI);
367     scaleVector(pfTemp, pfTemp, handle->scaleM);
368     addVector(pfDelta, pfDelta, pfTemp);
370     /* Rotate the I vector from the DCM matrix by the scaled rotation
371      *     rotation vector crossed with old I
372      *     old I += new I
373      */
374     crossProduct(pfI, pfDelta, handle->dcm[0]); 
375     addVector(handle->dcm[0], handle->dcm[0], pfI); 
377     /* Rotate the K vector from the DCM matrix by the scaled rotation
378      *     rotation vector crossed with old K
379      *     old K += new K
380      */
381     crossProduct(pfK, pfDelta, handle->dcm[2]); 
382     addVector(handle->dcm[2], handle->dcm[2], pfK); 
384     /* Compute the orthogonality error between the rotated I and K vectors and
385      * adjust each by half the error, bringing them closer to orthogonality
386      * cross couple to cancel out excess rotation:
387      *     I = I + err*K
388      *     K = K + err*I
389      */
390     fError = dotProduct(handle->dcm[0], handle->dcm[2]) / -2.0f;
391     scaleVector(pfI, handle->dcm[0], fError);
392     scaleVector(pfK, handle->dcm[2], fError);
393     addVector(handle->dcm[0], handle->dcm[0], pfK);  
394     addVector(handle->dcm[2], handle->dcm[2], pfI);
396     /* Normalize the I and K vectors.
397      *     Use the Taylor expansion around 1 to simplify calculations
398      */
399     scaleVector(handle->dcm[0], handle->dcm[0],
400                 0.5f * (3.0f - dotProduct(handle->dcm[0], handle->dcm[0])));
401     scaleVector(handle->dcm[2], handle->dcm[2],
402                 0.5f * (3.0f - dotProduct(handle->dcm[2], handle->dcm[2])));
404     /* Compute the rotated J vector from the cross product of the rotated,
405      * corrected K and I vectors 
406      */
407     crossProduct(handle->dcm[1], handle->dcm[2], handle->dcm[0]);
409     /* Determine if the newly updated DCM contains any invalid (i.e.,
410      * NaN) values 
411      */
412     bNAN = (isnan(handle->dcm[0][0]) || isnan(handle->dcm[0][1])
413             || isnan(handle->dcm[0][2]) || isnan(handle->dcm[1][0])
414             || isnan(handle->dcm[1][1]) || isnan(handle->dcm[1][2])
415             || isnan(handle->dcm[2][0]) || isnan(handle->dcm[2][1])
416             || isnan(handle->dcm[2][2]));
418     /* As a debug measure, we check for NaN in the DCM.  The user can trap
419      * this event depending on their implementation of __error__.  Should they
420      * choose to disable interrupts and loop forever then they will have
421      * preserved the stack and can analyze how they arrived at NaN 
422      */
423     assert(!bNAN);
425     /* If any part of the matrix is not-a-number then reset the DCM back to 
426      * the identity matrix 
427      */
428     if (bNAN) {
429         handle->dcm[0][0] = 1.0f;
430         handle->dcm[0][1] = 0.0f;
431         handle->dcm[0][2] = 0.0f;
432         handle->dcm[1][0] = 0.0f;
433         handle->dcm[1][1] = 1.0f;
434         handle->dcm[1][2] = 0.0f;
435         handle->dcm[2][0] = 0.0f;
436         handle->dcm[2][1] = 0.0f;
437         handle->dcm[2][2] = 1.0f;
438     }
441 /*
442  *  ======== DCM_getMatrix ========
443  *  Assigns the input matrix with the current DCM values.
444  * 
445  *  handle is a pointer to the DCM state structure.
446  *  dcm is a pointer to the array into which to store the DCM matrix
447  *  values.
448  */
449 void DCM_getMatrix(DCM_Handle handle, float dcm[3][3])
451     /* set the input matrix as the DCM */
452     dcm[0][0] = handle->dcm[0][0];
453     dcm[0][1] = handle->dcm[0][1];
454     dcm[0][2] = handle->dcm[0][2];
455     dcm[1][0] = handle->dcm[1][0];
456     dcm[1][1] = handle->dcm[1][1];
457     dcm[1][2] = handle->dcm[1][2];
458     dcm[2][0] = handle->dcm[2][0];
459     dcm[2][1] = handle->dcm[2][1];
460     dcm[2][2] = handle->dcm[2][2];
463 /*
464  *  ======== DCM_computeEulers ========
465  *  Computes the Euler angles from the DCM attitude estimation matrix.
466  * 
467  *  handle is a pointer to the DCM state structure.
468  *  pfRoll is a pointer to the value into which the roll is stored.
469  *  pfPitch is a pointer to the value into which the pitch is stored.
470  *  pfYaw is a pointer to the value into which the yaw is stored.
471  * 
472  *  This function computes the Euler angles that are represented by the DCM
473  *  attitude estimation matrix.  If any of the Euler angles is not required,
474  *  the corresponding parameter can be \b NULL.
475  */
476 void DCM_computeEulers(DCM_Handle handle,
477                        float *pfRoll, float *pfPitch,  float *pfYaw)
479     /* Compute the roll, pitch, and yaw as required */
480     if (pfRoll) {
481         *pfRoll = atan2f(handle->dcm[2][1], handle->dcm[2][2]);
482     }
483     if (pfPitch) {
484         *pfPitch = -asinf(handle->dcm[2][0]);
485     }
486     if (pfYaw) {
487         *pfYaw = atan2f(handle->dcm[1][0], handle->dcm[0][0]);
488     }
491 /*
492  *  ======== DCM_computeQuaternion ========
493  *  Computes the quaternion from the DCM attitude estimation matrix.
494  * 
495  *  handle is a pointer to the DCM state structure.
496  *  pfQuaternion is an array into which the quaternion is stored.
497  * 
498  *  This function computes the quaternion that is represented by the DCM
499  *  attitude estimation matrix.
500  */
501 void DCM_computeQuaternion(DCM_Handle handle, float pfQuaternion[4])
503     float fQs, fQx, fQy, fQz;
505     /* Partially compute Qs, Qx, Qy, and Qz based on the DCM diagonals.  The
506      * square root, an expensive operation, is computed for only one of these
507      * as determined later 
508      */
509     fQs = 1 + handle->dcm[0][0] + handle->dcm[1][1] + handle->dcm[2][2];
510     fQx = 1 + handle->dcm[0][0] - handle->dcm[1][1] - handle->dcm[2][2];
511     fQy = 1 - handle->dcm[0][0] + handle->dcm[1][1] - handle->dcm[2][2];
512     fQz = 1 - handle->dcm[0][0] - handle->dcm[1][1] + handle->dcm[2][2];
514     /* See if Qs is the largest of the diagonal values */
515     if ((fQs > fQx) && (fQs > fQy) && (fQs > fQz)) {
516         /* Finish the computation of Qs */
517         fQs = sqrtf(fQs) / 2;
519         /* Compute the values of the quaternion based on Qs */
520         pfQuaternion[0] = fQs;
521         pfQuaternion[1] = ((handle->dcm[2][1] - handle->dcm[1][2]) /
522                            (4 * fQs));
523         pfQuaternion[2] = ((handle->dcm[0][2] - handle->dcm[2][0]) /
524                            (4 * fQs));
525         pfQuaternion[3] = ((handle->dcm[1][0] - handle->dcm[0][1]) /
526                            (4 * fQs));
527     }
528     /* Qs is not the largest, so see if Qx is the largest remaining diagonal
529      * value 
530      */
531     else if ((fQx > fQy) && (fQx > fQz)) {
532         /* Finish the computation of Qx */
533         fQx = sqrtf(fQx) / 2;
535         /* Compute the values of the quaternion based on Qx */
536         pfQuaternion[0] = ((handle->dcm[2][1] - handle->dcm[1][2]) /
537                            (4 * fQx));
538         pfQuaternion[1] = fQx;
539         pfQuaternion[2] = ((handle->dcm[1][0] + handle->dcm[0][1]) /
540                            (4 * fQx));
541         pfQuaternion[3] = ((handle->dcm[0][2] + handle->dcm[2][0]) /
542                            (4 * fQx));
543     }
544     /* Qs and Qx are not the largest, so see if Qy is the largest remaining
545      * diagonal value 
546      */
547     else if (fQy > fQz)
548     {
549         /* Finish the computation of Qy */
550         fQy = sqrtf(fQy) / 2;
552         /* Compute the values of the quaternion based on Qy */
553         pfQuaternion[0] = ((handle->dcm[0][2] - handle->dcm[2][0]) /
554                            (4 * fQy));
555         pfQuaternion[1] = ((handle->dcm[1][0] + handle->dcm[0][1]) /
556                            (4 * fQy));
557         pfQuaternion[2] = fQy;
558         pfQuaternion[3] = ((handle->dcm[2][1] + handle->dcm[1][2]) /
559                            (4 * fQy));
560     }
561     /* Qz is the largest diagonal value */
562     else {
563         /* Finish the computation of Qz */
564         fQz = sqrtf(fQz) / 2;
566         /* Compute the values of the quaternion based on Qz */
567         pfQuaternion[0] = ((handle->dcm[1][0] - handle->dcm[0][1]) /
568                            (4 * fQz));
569         pfQuaternion[1] = ((handle->dcm[0][2] + handle->dcm[2][0]) /
570                            (4 * fQz));
571         pfQuaternion[2] = ((handle->dcm[2][1] + handle->dcm[1][2]) /
572                            (4 * fQz));
573         pfQuaternion[3] = fQz;
574     }
577 /*
578  *  ======== dotProduct ========
579  *  Computes the dot product of two vectors.
580  * 
581  *  v1 is the first vector.
582  *  v2 is the second vector.
583  * 
584  *  This function computes the dot product of two 3-dimensional vector.
585  * 
586  *  Returns the dot product of the two vectors.
587  */
588 static float dotProduct(float v1[3], float v2[3])
590     return ((v1[0] * v2[0]) + (v1[1] * v2[1]) + (v1[2] * v2[2]));
593 /*
594  *  ======== crossProduct ========
595  *  Computes the cross product of two vectors.
596  * 
597  *  vout is the output vector.
598  *  v1 is the first vector.
599  *  v2 is the second vector.
600  * 
601  *  This function computes the cross product of two 3-dimensional vectors.
602  */
603 static void crossProduct(float vout[3], float v1[3], float v2[3])
605     vout[0] = ((v1[1] * v2[2]) - (v1[2] * v2[1]));
606     vout[1] = ((v1[2] * v2[0]) - (v1[0] * v2[2]));
607     vout[2] = ((v1[0] * v2[1]) - (v1[1] * v2[0]));
610 /*
611  *  ======== scaleVector ========
612  *  Scales a vector.
613  * 
614  *  vout is the output vector.
615  *  vin is the input vector.
616  *  fScale is the scale factor.
617  * 
618  *  This function scales a 3-dimensional vector by multiplying each of its
619  *  components by the scale factor.
620  */
621 static void scaleVector(float vout[3], float vin[3], float fScale)
623     vout[0] = vin[0] * fScale;
624     vout[1] = vin[1] * fScale;
625     vout[2] = vin[2] * fScale;
628 /*
629  *  ======== addVector ========
630  *  Adds two vectors.
631  * 
632  *  vout is the output vector.
633  *  v1 is the first vector.
634  *  v2 is the second vector.
635  * 
636  *  This function adds two 3-dimensional vectors.
637  */
638 static void addVector(float vout[3], float v1[3], float v2[3])
640     vout[0] = v1[0] + v2[0];
641     vout[1] = v1[1] + v2[1];
642     vout[2] = v1[2] + v2[2];
645 /*
646  *  ======== normalize ========
647  *  Normalizes a vector
648  * 
649  *  vin is the input vector.
650  * 
651  *  This function normalizes a 3-dimensional vectors by dividing its
652  *  components by its magnitude.
653  */
654 static void normalize(float vin[3])
656     float magnitude = sqrtf(dotProduct(vin, vin));
657     vin[0] = (vin[0] / magnitude);
658     vin[1] = (vin[1] / magnitude);
659     vin[2] = (vin[2] / magnitude);