1 /****************************************************************************** 2 * $Id: AKFS_Direction.c 580 2012-03-29 09:56:21Z yamada.rj $ 3 ****************************************************************************** 4 * 5 * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan 6 * 7 * Licensed under the Apache License, Version 2.0 (the "License"); 8 * you may not use this file except in compliance with the License. 9 * You may obtain a copy of the License at 10 * 11 * http://www.apache.org/licenses/LICENSE-2.0 12 * 13 * Unless required by applicable law or agreed to in writing, software 14 * distributed under the License is distributed on an "AS IS" BASIS, 15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 * See the License for the specific language governing permissions and 17 * limitations under the License. 18 */ 19 #include "AKFS_Direction.h" 20 #include "AKFS_VNorm.h" 21 #include "AKFS_Math.h" 22 23 /* 24 Coordinate system is right-handed. 25 X-Axis: from left to right. 26 Y-Axis: from bottom to top. 27 Z-Axis: from reverse to obverse. 28 29 azimuth: Rotaion around Z axis, with positive values 30 when y-axis moves toward the x-axis. 31 pitch: Rotation around X axis, with positive values 32 when z-axis moves toward the y-axis. 33 roll: Rotation around Y axis, with positive values 34 when x-axis moves toward the z-axis. 35 */ 36 37 /* 38 This function is used internaly, so output is RADIAN! 39 */ 40 static void AKFS_Angle( 41 const AKFVEC* avec, 42 AKFLOAT* pitch, /* radian */ 43 AKFLOAT* roll /* radian */ 44 ) 45 { 46 AKFLOAT av; /* Size of vector */ 47 48 av = AKFS_SQRT((avec->u.x)*(avec->u.x) + (avec->u.y)*(avec->u.y) + (avec->u.z)*(avec->u.z)); 49 50 *pitch = AKFS_ASIN(-(avec->u.y) / av); 51 *roll = AKFS_ASIN((avec->u.x) / av); 52 } 53 54 /* 55 This function is used internaly, so output is RADIAN! 56 */ 57 static void AKFS_Azimuth( 58 const AKFVEC* hvec, 59 const AKFLOAT pitch, /* radian */ 60 const AKFLOAT roll, /* radian */ 61 AKFLOAT* azimuth /* radian */ 62 ) 63 { 64 AKFLOAT sinP; /* sin value of pitch angle */ 65 AKFLOAT cosP; /* cos value of pitch angle */ 66 AKFLOAT sinR; /* sin value of roll angle */ 67 AKFLOAT cosR; /* cos value of roll angle */ 68 AKFLOAT Xh; /* X axis element of vector which is projected to horizontal plane */ 69 AKFLOAT Yh; /* Y axis element of vector which is projected to horizontal plane */ 70 71 sinP = AKFS_SIN(pitch); 72 cosP = AKFS_COS(pitch); 73 sinR = AKFS_SIN(roll); 74 cosR = AKFS_COS(roll); 75 76 Yh = -(hvec->u.x)*cosR + (hvec->u.z)*sinR; 77 Xh = (hvec->u.x)*sinP*sinR + (hvec->u.y)*cosP + (hvec->u.z)*sinP*cosR; 78 79 /* atan2(y, x) -> divisor and dividend is opposite from mathematical equation. */ 80 *azimuth = AKFS_ATAN2(Yh, Xh); 81 } 82 83 int16 AKFS_Direction( 84 const int16 nhvec, 85 const AKFVEC hvec[], 86 const int16 hnave, 87 const int16 navec, 88 const AKFVEC avec[], 89 const int16 anave, 90 AKFLOAT* azimuth, 91 AKFLOAT* pitch, 92 AKFLOAT* roll 93 ) 94 { 95 AKFVEC have, aave; 96 AKFLOAT azimuthRad; 97 AKFLOAT pitchRad; 98 AKFLOAT rollRad; 99 100 /* arguments check */ 101 if ((nhvec <= 0) || (navec <= 0) || (hnave <= 0) || (anave <= 0)) { 102 return AKFS_ERROR; 103 } 104 if ((nhvec < hnave) || (navec < anave)) { 105 return AKFS_ERROR; 106 } 107 108 /* average */ 109 if (AKFS_VbAve(nhvec, hvec, hnave, &have) != AKFS_SUCCESS) { 110 return AKFS_ERROR; 111 } 112 if (AKFS_VbAve(navec, avec, anave, &aave) != AKFS_SUCCESS) { 113 return AKFS_ERROR; 114 } 115 116 /* calculate pitch and roll */ 117 AKFS_Angle(&aave, &pitchRad, &rollRad); 118 119 /* calculate azimuth */ 120 AKFS_Azimuth(&have, pitchRad, rollRad, &azimuthRad); 121 122 *azimuth = RAD2DEG(azimuthRad); 123 *pitch = RAD2DEG(pitchRad); 124 *roll = RAD2DEG(rollRad); 125 126 /* Adjust range of azimuth */ 127 if (*azimuth < 0) { 128 *azimuth += 360.0f; 129 } 130 131 return AKFS_SUCCESS; 132 } 133 134 135
