1 /* 2 * Copyright (C) 2016 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include <algos/quat.h> 18 #include <nanohub_math.h> 19 20 static float clamp(float x) { 21 return x < 0.0f ? 0.0f : x; 22 } 23 24 void initQuat(Quat *q, const struct Mat33 *R) { 25 float Hx = R->elem[0][0]; 26 float My = R->elem[1][1]; 27 float Az = R->elem[2][2]; 28 29 q->x = sqrtf(clamp(Hx - My - Az + 1.0f) * 0.25f); 30 q->y = sqrtf(clamp(-Hx + My - Az + 1.0f) * 0.25f); 31 q->z = sqrtf(clamp(-Hx - My + Az + 1.0f) * 0.25f); 32 q->w = sqrtf(clamp(Hx + My + Az + 1.0f) * 0.25f); 33 q->x = copysignf(q->x, R->elem[1][2] - R->elem[2][1]); 34 q->y = copysignf(q->y, R->elem[2][0] - R->elem[0][2]); 35 q->z = copysignf(q->z, R->elem[0][1] - R->elem[1][0]); 36 } 37 38 void quatToMatrix(struct Mat33 *R, const Quat *q) { 39 float q0 = q->w; 40 float q1 = q->x; 41 float q2 = q->y; 42 float q3 = q->z; 43 float sq_q1 = 2.0f * q1 * q1; 44 float sq_q2 = 2.0f * q2 * q2; 45 float sq_q3 = 2.0f * q3 * q3; 46 float q1_q2 = 2.0f * q1 * q2; 47 float q3_q0 = 2.0f * q3 * q0; 48 float q1_q3 = 2.0f * q1 * q3; 49 float q2_q0 = 2.0f * q2 * q0; 50 float q2_q3 = 2.0f * q2 * q3; 51 float q1_q0 = 2.0f * q1 * q0; 52 53 R->elem[0][0] = 1.0f - sq_q2 - sq_q3; 54 R->elem[1][0] = q1_q2 - q3_q0; 55 R->elem[2][0] = q1_q3 + q2_q0; 56 R->elem[0][1] = q1_q2 + q3_q0; 57 R->elem[1][1] = 1.0f - sq_q1 - sq_q3; 58 R->elem[2][1] = q2_q3 - q1_q0; 59 R->elem[0][2] = q1_q3 - q2_q0; 60 R->elem[1][2] = q2_q3 + q1_q0; 61 R->elem[2][2] = 1.0f - sq_q1 - sq_q2; 62 } 63 64 void quatNormalize(Quat *q) { 65 if (q->w < 0.0f) { 66 q->x = -q->x; 67 q->y = -q->y; 68 q->z = -q->z; 69 q->w = -q->w; 70 } 71 72 float invNorm = 73 1.0f / sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w); 74 75 q->x *= invNorm; 76 q->y *= invNorm; 77 q->z *= invNorm; 78 q->w *= invNorm; 79 } 80 81
