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 * ======== 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)
223 {
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];
263 }
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)
282 {
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];
289 /* A temporary storage vector */
290 float pfTemp[3];
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 }
419 }
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])
430 {
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];
441 }
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)
458 {
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 }
469 }
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])
482 {
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 }
555 }
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])
569 {
570 return ((v1[0] * v2[0]) + (v1[1] * v2[1]) + (v1[2] * v2[2]));
571 }
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])
584 {
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]));
588 }
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)
602 {
603 vout[0] = vin[0] * fScale;
604 vout[1] = vin[1] * fScale;
605 vout[2] = vin[2] * fScale;
606 }
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])
619 {
620 vout[0] = v1[0] + v2[0];
621 vout[1] = v1[1] + v2[1];
622 vout[2] = v1[2] + v2[2];
623 }
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])
635 {
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);
640 }