1 /*
2 * Copyright (c) 2021 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 * http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15
16 #include "base/geometry/quaternion.h"
17
18 #include <cmath>
19
20 namespace OHOS::Ace {
21 namespace {
22
23 constexpr double KEPSILON = 1e-5;
24
25 } // namespace
26
Slerp(const Quaternion & to,double t) const27 Quaternion Quaternion::Slerp(const Quaternion& to, double t) const
28 {
29 if (t < 0.0 || t > 1.0) {
30 // https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
31 // a scalar between 0.0 (at from) and 1.0 (at to)
32 return *this;
33 }
34
35 Quaternion from = *this;
36
37 double cosHalfAngle = from.x_ * to.x_ + from.y_ * to.y_ + from.z_ * to.z_ + from.w_ * to.w_;
38 if (cosHalfAngle < 0.0) {
39 // Since the half angle is > 90 degrees, the full rotation angle would
40 // exceed 180 degrees. The quaternions (x, y, z, w) and (-x, -y, -z, -w)
41 // represent the same rotation. Flipping the orientation of either
42 // quaternion ensures that the half angle is less than 90 and that we are
43 // taking the shortest path.
44 from = from.flip();
45 cosHalfAngle = -cosHalfAngle;
46 }
47
48 // Ensure that acos is well behaved at the boundary.
49 if (cosHalfAngle > 1.0) {
50 cosHalfAngle = 1.0;
51 }
52
53 double sinHalfAngle = std::sqrt(1.0 - cosHalfAngle * cosHalfAngle);
54 if (sinHalfAngle < KEPSILON) {
55 // Quaternions share common axis and angle.
56 return *this;
57 }
58
59 double half_angle = std::acos(cosHalfAngle);
60
61 double scaleA = std::sin((1.0 - t) * half_angle) / sinHalfAngle;
62 double scaleB = std::sin(t * half_angle) / sinHalfAngle;
63
64 return (scaleA * from) + (scaleB * to);
65 }
66
67 } // namespace OHOS::Ace
68