Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2189 - 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);