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 */
AKFS_Angle(const AKFVEC * avec,AKFLOAT * pitch,AKFLOAT * roll)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 */
AKFS_Azimuth(const AKFVEC * hvec,const AKFLOAT pitch,const AKFLOAT roll,AKFLOAT * azimuth)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
AKFS_Direction(const int16 nhvec,const AKFVEC hvec[],const int16 hnave,const int16 navec,const AKFVEC avec[],const int16 anave,AKFLOAT * azimuth,AKFLOAT * pitch,AKFLOAT * roll)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