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 */
40static 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 */
57static 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
83int16 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