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;
112 }
114 /*
115 * ======== DCM_delete ========
116 */
117 void DCM_delete(DCM_Handle handle)
118 {
119 free(handle);
120 }
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)
139 {
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;
149 }
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)
168 {
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;
178 }
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)
197 {
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;
207 }
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)
218 {
219 /* two correction vectors fed into the controller */
220 float pfKError[3], pfIError[3];
222 /* calcluate the Z-axis correction based on accelerometer reading */
223 crossProduct(pfKError, handle->dcm[2], handle->pfAccel);
225 /* calcluate the X-axis correction based on magnetometer reading */
226 crossProduct(pfIError, handle->dcm[0], handle->pfMagneto);
227 }
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)
243 {
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];
283 }
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)
302 {
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];
309 /* A temporary storage vector */
310 float pfTemp[3];
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 }
439 }
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])
450 {
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];
461 }
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)
478 {
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 }
489 }
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])
502 {
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 }
575 }
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])
589 {
590 return ((v1[0] * v2[0]) + (v1[1] * v2[1]) + (v1[2] * v2[2]));
591 }
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])
604 {
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]));
608 }
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)
622 {
623 vout[0] = vin[0] * fScale;
624 vout[1] = vin[1] * fScale;
625 vout[2] = vin[2] * fScale;
626 }
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])
639 {
640 vout[0] = v1[0] + v2[0];
641 vout[1] = v1[1] + v2[1];
642 vout[2] = v1[2] + v2[2];
643 }
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])
655 {
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);
660 }