Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1702 | - | 1 | /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
| 2 | /* |
||
| 3 | * vector3.cpp |
||
| 4 | * Copyright (C) Andrew Tridgell 2012 |
||
| 5 | * |
||
| 6 | * This file is free software: you can redistribute it and/or modify it |
||
| 7 | * under the terms of the GNU General Public License as published by the |
||
| 8 | * Free Software Foundation, either version 3 of the License, or |
||
| 9 | * (at your option) any later version. |
||
| 10 | * |
||
| 11 | * This file is distributed in the hope that it will be useful, but |
||
| 12 | * WITHOUT ANY WARRANTY; without even the implied warranty of |
||
| 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||
| 14 | * See the GNU General Public License for more details. |
||
| 15 | * |
||
| 16 | * You should have received a copy of the GNU General Public License along |
||
| 17 | * with this program. If not, see <http://www.gnu.org/licenses/>. |
||
| 18 | */ |
||
| 19 | |||
| 20 | #include "AP_Math.h" |
||
| 21 | |||
| 22 | #define HALF_SQRT_2 0.70710678118654757 |
||
| 23 | |||
| 24 | // rotate a vector by a standard rotation, attempting |
||
| 25 | // to use the minimum number of floating point operations |
||
| 26 | template <typename T> |
||
| 27 | void Vector3<T>::rotate(enum Rotation rotation) |
||
| 28 | { |
||
| 29 | T tmp; |
||
| 30 | switch (rotation) { |
||
| 31 | case ROTATION_NONE: |
||
| 32 | case ROTATION_MAX: |
||
| 33 | return; |
||
| 34 | case ROTATION_YAW_45: { |
||
| 35 | tmp = HALF_SQRT_2*(x - y); |
||
| 36 | y = HALF_SQRT_2*(x + y); |
||
| 37 | x = tmp; |
||
| 38 | return; |
||
| 39 | } |
||
| 40 | case ROTATION_YAW_90: { |
||
| 41 | tmp = x; x = -y; y = tmp; |
||
| 42 | return; |
||
| 43 | } |
||
| 44 | case ROTATION_YAW_135: { |
||
| 45 | tmp = -HALF_SQRT_2*(x + y); |
||
| 46 | y = HALF_SQRT_2*(x - y); |
||
| 47 | x = tmp; |
||
| 48 | return; |
||
| 49 | } |
||
| 50 | case ROTATION_YAW_180: |
||
| 51 | x = -x; y = -y; |
||
| 52 | return; |
||
| 53 | case ROTATION_YAW_225: { |
||
| 54 | tmp = HALF_SQRT_2*(y - x); |
||
| 55 | y = -HALF_SQRT_2*(x + y); |
||
| 56 | x = tmp; |
||
| 57 | return; |
||
| 58 | } |
||
| 59 | case ROTATION_YAW_270: { |
||
| 60 | tmp = x; x = y; y = -tmp; |
||
| 61 | return; |
||
| 62 | } |
||
| 63 | case ROTATION_YAW_315: { |
||
| 64 | tmp = HALF_SQRT_2*(x + y); |
||
| 65 | y = HALF_SQRT_2*(y - x); |
||
| 66 | x = tmp; |
||
| 67 | return; |
||
| 68 | } |
||
| 69 | case ROTATION_ROLL_180: { |
||
| 70 | y = -y; z = -z; |
||
| 71 | return; |
||
| 72 | } |
||
| 73 | case ROTATION_ROLL_180_YAW_45: { |
||
| 74 | tmp = HALF_SQRT_2*(x + y); |
||
| 75 | y = HALF_SQRT_2*(x - y); |
||
| 76 | x = tmp; z = -z; |
||
| 77 | return; |
||
| 78 | } |
||
| 79 | case ROTATION_ROLL_180_YAW_90: { |
||
| 80 | tmp = x; x = y; y = tmp; z = -z; |
||
| 81 | return; |
||
| 82 | } |
||
| 83 | case ROTATION_ROLL_180_YAW_135: { |
||
| 84 | tmp = HALF_SQRT_2*(y - x); |
||
| 85 | y = HALF_SQRT_2*(y + x); |
||
| 86 | x = tmp; z = -z; |
||
| 87 | return; |
||
| 88 | } |
||
| 89 | case ROTATION_PITCH_180: { |
||
| 90 | x = -x; z = -z; |
||
| 91 | return; |
||
| 92 | } |
||
| 93 | case ROTATION_ROLL_180_YAW_225: { |
||
| 94 | tmp = -HALF_SQRT_2*(x + y); |
||
| 95 | y = HALF_SQRT_2*(y - x); |
||
| 96 | x = tmp; z = -z; |
||
| 97 | return; |
||
| 98 | } |
||
| 99 | case ROTATION_ROLL_180_YAW_270: { |
||
| 100 | tmp = x; x = -y; y = -tmp; z = -z; |
||
| 101 | return; |
||
| 102 | } |
||
| 103 | case ROTATION_ROLL_180_YAW_315: { |
||
| 104 | tmp = HALF_SQRT_2*(x - y); |
||
| 105 | y = -HALF_SQRT_2*(x + y); |
||
| 106 | x = tmp; z = -z; |
||
| 107 | return; |
||
| 108 | } |
||
| 109 | } |
||
| 110 | } |
||
| 111 | |||
| 112 | // only define for signed numbers |
||
| 113 | template void Vector3<float>::rotate(enum Rotation); |
||
| 114 | template void Vector3<int16_t>::rotate(enum Rotation); |
||
| 115 | template void Vector3<int32_t>::rotate(enum Rotation); |