~ubuntu-branches/ubuntu/trusty/travis/trusty

« back to all changes in this revision

Viewing changes to src/xvector3.h

  • Committer: Package Import Robot
  • Author(s): Daniel Leidert
  • Date: 2014-01-18 20:07:16 UTC
  • mfrom: (1.1.8)
  • Revision ID: package-import@ubuntu.com-20140118200716-whsmcg7fa1eyqecq
Tags: 140117-1
New upstream release.

Show diffs side-by-side

added added

removed removed

Lines of Context:
1
 
/*****************************************************************************
2
 
    TRAVIS - Trajectory Analyzer and Visualizer
3
 
    http://www.travis-analyzer.de/
4
 
 
5
 
    Copyright (c) 2009-2013 Martin Brehm
6
 
                  2012-2013 Martin Thomas
7
 
 
8
 
    This file written by Martin Brehm.
9
 
 
10
 
    This program is free software: you can redistribute it and/or modify
11
 
    it under the terms of the GNU General Public License as published by
12
 
    the Free Software Foundation, either version 3 of the License, or
13
 
    (at your option) any later version.
14
 
 
15
 
    This program is distributed in the hope that it will be useful,
16
 
    but WITHOUT ANY WARRANTY; without even the implied warranty of
17
 
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18
 
    GNU General Public License for more details.
19
 
 
20
 
    You should have received a copy of the GNU General Public License
21
 
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
22
 
*****************************************************************************/
23
 
 
24
 
 
25
 
#ifndef CXVECTOR3_DEFINED
26
 
#define CXVECTOR3_DEFINED
27
 
 
28
 
#include "tools.h"
29
 
#include "xobject.h"
30
 
#include "backtrace.h"
31
 
#include "xdvector3.h"
32
 
 
33
 
 
34
 
class CxVector3 : public CxObject
35
 
{
36
 
public:
37
 
        inline CxVector3() { }
38
 
 
39
 
        inline ~CxVector3() { }
40
 
 
41
 
/*      CxVector3(CxVector3 v)
42
 
        {
43
 
                m_pData[0] = v.m_pData[0];
44
 
                m_pData[1] = v.m_pData[1];
45
 
                m_pData[2] = v.m_pData[2];
46
 
        }*/
47
 
        
48
 
        inline CxVector3(const CxVector3 &v) : CxObject()
49
 
        {
50
 
                BXIN;
51
 
                m_pData[0] = v.m_pData[0];
52
 
                m_pData[1] = v.m_pData[1];
53
 
                m_pData[2] = v.m_pData[2];
54
 
                BXOUT;
55
 
        }
56
 
 
57
 
        inline CxVector3(const CxDVector3 &v)
58
 
        {
59
 
                BXIN;
60
 
                m_pData[0] = (float)v.GetAt(0);
61
 
                m_pData[1] = (float)v.GetAt(1);
62
 
                m_pData[2] = (float)v.GetAt(2);
63
 
                BXOUT;
64
 
        }
65
 
 
66
 
        inline CxVector3(float f)
67
 
        {
68
 
                BXIN;
69
 
                m_pData[0] = f;
70
 
                m_pData[1] = f;
71
 
                m_pData[2] = f;
72
 
                BXOUT;
73
 
        }
74
 
 
75
 
        inline CxVector3(float x, float y, float z)
76
 
        {
77
 
                BXIN;
78
 
                m_pData[0] = x;
79
 
                m_pData[1] = y;
80
 
                m_pData[2] = z;
81
 
                BXOUT;
82
 
        }
83
 
 
84
 
        inline float &GetAt(int i)
85
 
        {
86
 
                #ifdef DEBUG_CVECTOR3
87
 
                mprintf("@ CxVector3::GetAt(int): %d...",i);
88
 
                #endif
89
 
                return m_pData[i];
90
 
        }
91
 
 
92
 
        inline float &operator [] (int i)
93
 
        {
94
 
                #ifdef DEBUG_CVECTOR3
95
 
                mprintf("@ CxVector3::operator [] (int): %d\n",i);
96
 
                #endif
97
 
                return GetAt(i);
98
 
        }
99
 
 
100
 
        inline float GetAt(int i) const
101
 
        {
102
 
                #ifdef DEBUG_CVECTOR3
103
 
                mprintf("@ CxVector3::GetAt(int): %d...",i);
104
 
                #endif
105
 
                #ifdef DEBUG_ARRAYS
106
 
                if (i > 2)
107
 
                {
108
 
                        mprintf("CxVector3::GetAt(int): Boundary Error (%d/3)...",i);
109
 
                        abort();
110
 
                }
111
 
                #endif
112
 
                return m_pData[i];
113
 
        }
114
 
 
115
 
        inline float operator [] (int i) const
116
 
        {
117
 
                #ifdef DEBUG_CVECTOR3
118
 
                mprintf("@ CxVector3::operator [] (int): %d\n",i);
119
 
                #endif
120
 
                return GetAt(i);
121
 
        }
122
 
 
123
 
        inline CxVector3 operator + (const CxVector3 &v) const
124
 
        {
125
 
                #ifdef DEBUG_CVECTOR3
126
 
                mprintf("@ CxVector3::operator + (CxVector3&)\n");
127
 
                #endif
128
 
                return CxVector3(m_pData[0]+v.m_pData[0],m_pData[1]+v.m_pData[1],m_pData[2]+v.m_pData[2]);
129
 
        }
130
 
 
131
 
        inline CxVector3 operator - (const CxVector3 &v) const
132
 
        {
133
 
                #ifdef DEBUG_CVECTOR3
134
 
                mprintf("@ CxVector3::operator - (CxVector3&)\n");
135
 
                #endif
136
 
                return CxVector3(m_pData[0]-v.m_pData[0],m_pData[1]-v.m_pData[1],m_pData[2]-v.m_pData[2]);
137
 
        }
138
 
 
139
 
        inline CxVector3 operator * (const float &f) const
140
 
        {
141
 
                #ifdef DEBUG_CVECTOR3
142
 
                mprintf("@ CxVector3::operator * (float)\n");
143
 
                #endif
144
 
                return CxVector3(m_pData[0]*f,m_pData[1]*f,m_pData[2]*f);
145
 
        }
146
 
 
147
 
        inline CxVector3 operator / (const float &f) const
148
 
        {
149
 
                #ifdef DEBUG_CVECTOR3
150
 
                mprintf("@ CxVector3::operator / (float)\n");
151
 
                #endif
152
 
                return CxVector3(m_pData[0]/f,m_pData[1]/f,m_pData[2]/f);
153
 
        }
154
 
 
155
 
        inline void operator += (const CxVector3 &v)
156
 
        {
157
 
                BXIN;
158
 
                #ifdef DEBUG_CVECTOR3
159
 
                mprintf("@ CxVector3::operator += (CxVector3&)\n");
160
 
                #endif
161
 
                m_pData[0] += v.m_pData[0];
162
 
                m_pData[1] += v.m_pData[1];
163
 
                m_pData[2] += v.m_pData[2];
164
 
                BXOUT;
165
 
        }
166
 
 
167
 
        inline void operator -= (const CxVector3 &v)
168
 
        {
169
 
                BXIN;
170
 
                #ifdef DEBUG_CVECTOR3
171
 
                mprintf("@ CxVector3::operator -= (CxVector3&)\n");
172
 
                #endif
173
 
                m_pData[0] -= v.m_pData[0];
174
 
                m_pData[1] -= v.m_pData[1];
175
 
                m_pData[2] -= v.m_pData[2];
176
 
                BXOUT;
177
 
        }
178
 
 
179
 
        inline void operator *= (const float &f)
180
 
        {
181
 
                BXIN;
182
 
                #ifdef DEBUG_CVECTOR3
183
 
                mprintf("@ CxVector3::operator *= (float)\n");
184
 
                #endif
185
 
                m_pData[0] *= f;
186
 
                m_pData[1] *= f;
187
 
                m_pData[2] *= f;
188
 
                BXOUT;
189
 
        }
190
 
 
191
 
        inline void operator /= (const float &f)
192
 
        {
193
 
                BXIN;
194
 
                #ifdef DEBUG_CVECTOR3
195
 
                mprintf("@ CxVector3::operator /= (float)\n");
196
 
                #endif
197
 
                m_pData[0] /= f;
198
 
                m_pData[1] /= f;
199
 
                m_pData[2] /= f;
200
 
                BXOUT;
201
 
        }
202
 
 
203
 
        inline float GetLength() const
204
 
        {
205
 
                return (float)sqrt(m_pData[0]*m_pData[0]+m_pData[1]*m_pData[1]+m_pData[2]*m_pData[2]);
206
 
        }
207
 
 
208
 
        inline float GetLengthSqr() const
209
 
        {
210
 
                return (float)(m_pData[0]*m_pData[0]+m_pData[1]*m_pData[1]+m_pData[2]*m_pData[2]);
211
 
        }
212
 
 
213
 
        inline void Normalize()
214
 
        {
215
 
                BXIN;
216
 
                float l;
217
 
                l = GetLength();
218
 
                m_pData[0] /= l;
219
 
                m_pData[1] /= l;
220
 
                m_pData[2] /= l;
221
 
                BXOUT;
222
 
        }
223
 
 
224
 
        void PointRoot(const CxVector3 &vec1, const CxVector3 &vec2, const CxVector3 &point);
225
 
 
226
 
        void Dump() const;
227
 
 
228
 
private:
229
 
        float m_pData[3];
230
 
};
231
 
 
232
 
 
233
 
float Dihedral(const CxVector3 &vec1, const CxVector3 &vec2, const CxVector3 &norm, bool absolute);
234
 
 
235
 
float Dihedral2(const CxVector3 &vec1, const CxVector3 &vec2, const CxVector3 &norm, bool absolute);
236
 
 
237
 
 
238
 
inline void Swap(CxVector3 &vec1, CxVector3 &vec2)
239
 
{
240
 
        BXIN;
241
 
        CxVector3 t;
242
 
        t = vec1;
243
 
        vec1 = vec2;
244
 
        vec2 = t;
245
 
        BXOUT;
246
 
}
247
 
 
248
 
 
249
 
inline CxVector3 operator * (const float &f, const CxVector3 &v)
250
 
{
251
 
        return v*f;
252
 
}
253
 
 
254
 
 
255
 
inline CxVector3 CrossP(const CxVector3 &vec1, const CxVector3 &vec2)
256
 
{
257
 
        return CxVector3(vec1[1]*vec2[2] - vec1[2]*vec2[1],
258
 
                vec1[2]*vec2[0] - vec1[0]*vec2[2],
259
 
                vec1[0]*vec2[1] - vec1[1]*vec2[0]);
260
 
}
261
 
 
262
 
 
263
 
inline float DotP(const CxVector3 &vec1, const CxVector3 &vec2)
264
 
{
265
 
        return vec1[0]*vec2[0]+vec1[1]*vec2[1]+vec1[2]*vec2[2];
266
 
}
267
 
 
268
 
 
269
 
inline float Angle(const CxVector3 &vec1, const CxVector3 &vec2)
270
 
{
271
 
        BXIN;
272
 
        float t;
273
 
        t = DotP(vec1,vec2) / vec1.GetLength() / vec2.GetLength();
274
 
        if (t > 1.0f)
275
 
                t = 1.0f;
276
 
        if (t < -1.0f)
277
 
                t = -1.0f;
278
 
        return (float)acos(t);
279
 
        BXOUT;
280
 
}
281
 
 
282
 
 
283
 
inline float Angle_Deg(const CxVector3 &vec1, const CxVector3 &vec2)
284
 
{
285
 
        BXIN;
286
 
        float t;
287
 
        t = DotP(vec1,vec2) / vec1.GetLength() / vec2.GetLength();
288
 
        if (t > 1.0f)
289
 
                t = 1.0f;
290
 
        if (t < -1.0f)
291
 
                t = -1.0f;
292
 
        t = (float)acos(t);
293
 
        BXOUT;
294
 
        return (float)fabs(t*180.0f / Pi);
295
 
}
296
 
 
297
 
 
298
 
inline float VecDist(const CxVector3 &vec1, const CxVector3 &vec2)
299
 
{
300
 
        return (float)sqrt((vec1[0]-vec2[0])*(vec1[0]-vec2[0]) + (vec1[1]-vec2[1])*(vec1[1]-vec2[1]) + (vec1[2]-vec2[2])*(vec1[2]-vec2[2]));
301
 
}
302
 
 
303
 
 
304
 
CxVector3 PointFromRAD(CxVector3 r1, CxVector3 r2, CxVector3 r3, float r, float a, float d);
305
 
 
306
 
#endif
 
1
/*****************************************************************************
 
2
    TRAVIS - Trajectory Analyzer and Visualizer
 
3
    http://www.travis-analyzer.de/
 
4
 
 
5
    Copyright (c) 2009-2014 Martin Brehm
 
6
                  2012-2014 Martin Thomas
 
7
 
 
8
    This file written by Martin Brehm.
 
9
 
 
10
    This program is free software: you can redistribute it and/or modify
 
11
    it under the terms of the GNU General Public License as published by
 
12
    the Free Software Foundation, either version 3 of the License, or
 
13
    (at your option) any later version.
 
14
 
 
15
    This program is distributed in the hope that it will be useful,
 
16
    but WITHOUT ANY WARRANTY; without even the implied warranty of
 
17
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 
18
    GNU General Public License for more details.
 
19
 
 
20
    You should have received a copy of the GNU General Public License
 
21
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
22
*****************************************************************************/
 
23
 
 
24
 
 
25
#ifndef CXVECTOR3_DEFINED
 
26
#define CXVECTOR3_DEFINED
 
27
 
 
28
#include "tools.h"
 
29
#include "xobject.h"
 
30
#include "backtrace.h"
 
31
#include "xdvector3.h"
 
32
 
 
33
 
 
34
class CxVector3 : public CxObject
 
35
{
 
36
public:
 
37
        inline CxVector3() { }
 
38
 
 
39
        inline ~CxVector3() { }
 
40
 
 
41
/*      CxVector3(CxVector3 v)
 
42
        {
 
43
                m_pData[0] = v.m_pData[0];
 
44
                m_pData[1] = v.m_pData[1];
 
45
                m_pData[2] = v.m_pData[2];
 
46
        }*/
 
47
        
 
48
        inline CxVector3(const CxVector3 &v) : CxObject()
 
49
        {
 
50
                BXIN;
 
51
                m_pData[0] = v.m_pData[0];
 
52
                m_pData[1] = v.m_pData[1];
 
53
                m_pData[2] = v.m_pData[2];
 
54
                BXOUT;
 
55
        }
 
56
 
 
57
        inline CxVector3(const CxDVector3 &v)
 
58
        {
 
59
                BXIN;
 
60
                m_pData[0] = (float)v.GetAt(0);
 
61
                m_pData[1] = (float)v.GetAt(1);
 
62
                m_pData[2] = (float)v.GetAt(2);
 
63
                BXOUT;
 
64
        }
 
65
 
 
66
        inline CxVector3(float f)
 
67
        {
 
68
                BXIN;
 
69
                m_pData[0] = f;
 
70
                m_pData[1] = f;
 
71
                m_pData[2] = f;
 
72
                BXOUT;
 
73
        }
 
74
 
 
75
        inline CxVector3(float x, float y, float z)
 
76
        {
 
77
                BXIN;
 
78
                m_pData[0] = x;
 
79
                m_pData[1] = y;
 
80
                m_pData[2] = z;
 
81
                BXOUT;
 
82
        }
 
83
 
 
84
        inline float &GetAt(int i)
 
85
        {
 
86
                #ifdef DEBUG_CVECTOR3
 
87
                mprintf("@ CxVector3::GetAt(int): %d...",i);
 
88
                #endif
 
89
                return m_pData[i];
 
90
        }
 
91
 
 
92
        inline float &operator [] (int i)
 
93
        {
 
94
                #ifdef DEBUG_CVECTOR3
 
95
                mprintf("@ CxVector3::operator [] (int): %d\n",i);
 
96
                #endif
 
97
                return GetAt(i);
 
98
        }
 
99
 
 
100
        inline float GetAt(int i) const
 
101
        {
 
102
                #ifdef DEBUG_CVECTOR3
 
103
                mprintf("@ CxVector3::GetAt(int): %d...",i);
 
104
                #endif
 
105
                #ifdef DEBUG_ARRAYS
 
106
                if (i > 2)
 
107
                {
 
108
                        mprintf("CxVector3::GetAt(int): Boundary Error (%d/3)...",i);
 
109
                        abort();
 
110
                }
 
111
                #endif
 
112
                return m_pData[i];
 
113
        }
 
114
 
 
115
        inline float operator [] (int i) const
 
116
        {
 
117
                #ifdef DEBUG_CVECTOR3
 
118
                mprintf("@ CxVector3::operator [] (int): %d\n",i);
 
119
                #endif
 
120
                return GetAt(i);
 
121
        }
 
122
 
 
123
        inline CxVector3 operator + (const CxVector3 &v) const
 
124
        {
 
125
                #ifdef DEBUG_CVECTOR3
 
126
                mprintf("@ CxVector3::operator + (CxVector3&)\n");
 
127
                #endif
 
128
                return CxVector3(m_pData[0]+v.m_pData[0],m_pData[1]+v.m_pData[1],m_pData[2]+v.m_pData[2]);
 
129
        }
 
130
 
 
131
        inline CxVector3 operator - (const CxVector3 &v) const
 
132
        {
 
133
                #ifdef DEBUG_CVECTOR3
 
134
                mprintf("@ CxVector3::operator - (CxVector3&)\n");
 
135
                #endif
 
136
                return CxVector3(m_pData[0]-v.m_pData[0],m_pData[1]-v.m_pData[1],m_pData[2]-v.m_pData[2]);
 
137
        }
 
138
 
 
139
        inline CxVector3 operator * (const float &f) const
 
140
        {
 
141
                #ifdef DEBUG_CVECTOR3
 
142
                mprintf("@ CxVector3::operator * (float)\n");
 
143
                #endif
 
144
                return CxVector3(m_pData[0]*f,m_pData[1]*f,m_pData[2]*f);
 
145
        }
 
146
 
 
147
        inline CxVector3 operator / (const float &f) const
 
148
        {
 
149
                #ifdef DEBUG_CVECTOR3
 
150
                mprintf("@ CxVector3::operator / (float)\n");
 
151
                #endif
 
152
                return CxVector3(m_pData[0]/f,m_pData[1]/f,m_pData[2]/f);
 
153
        }
 
154
 
 
155
        inline void operator += (const CxVector3 &v)
 
156
        {
 
157
                BXIN;
 
158
                #ifdef DEBUG_CVECTOR3
 
159
                mprintf("@ CxVector3::operator += (CxVector3&)\n");
 
160
                #endif
 
161
                m_pData[0] += v.m_pData[0];
 
162
                m_pData[1] += v.m_pData[1];
 
163
                m_pData[2] += v.m_pData[2];
 
164
                BXOUT;
 
165
        }
 
166
 
 
167
        inline void operator -= (const CxVector3 &v)
 
168
        {
 
169
                BXIN;
 
170
                #ifdef DEBUG_CVECTOR3
 
171
                mprintf("@ CxVector3::operator -= (CxVector3&)\n");
 
172
                #endif
 
173
                m_pData[0] -= v.m_pData[0];
 
174
                m_pData[1] -= v.m_pData[1];
 
175
                m_pData[2] -= v.m_pData[2];
 
176
                BXOUT;
 
177
        }
 
178
 
 
179
        inline void operator *= (const float &f)
 
180
        {
 
181
                BXIN;
 
182
                #ifdef DEBUG_CVECTOR3
 
183
                mprintf("@ CxVector3::operator *= (float)\n");
 
184
                #endif
 
185
                m_pData[0] *= f;
 
186
                m_pData[1] *= f;
 
187
                m_pData[2] *= f;
 
188
                BXOUT;
 
189
        }
 
190
 
 
191
        inline void operator /= (const float &f)
 
192
        {
 
193
                BXIN;
 
194
                #ifdef DEBUG_CVECTOR3
 
195
                mprintf("@ CxVector3::operator /= (float)\n");
 
196
                #endif
 
197
                m_pData[0] /= f;
 
198
                m_pData[1] /= f;
 
199
                m_pData[2] /= f;
 
200
                BXOUT;
 
201
        }
 
202
 
 
203
        inline float GetLength() const
 
204
        {
 
205
                return (float)sqrt(m_pData[0]*m_pData[0]+m_pData[1]*m_pData[1]+m_pData[2]*m_pData[2]);
 
206
        }
 
207
 
 
208
        inline float GetLengthSqr() const
 
209
        {
 
210
                return (float)(m_pData[0]*m_pData[0]+m_pData[1]*m_pData[1]+m_pData[2]*m_pData[2]);
 
211
        }
 
212
 
 
213
        inline void Normalize()
 
214
        {
 
215
                BXIN;
 
216
                float l;
 
217
                l = GetLength();
 
218
                m_pData[0] /= l;
 
219
                m_pData[1] /= l;
 
220
                m_pData[2] /= l;
 
221
                BXOUT;
 
222
        }
 
223
 
 
224
        void PointRoot(const CxVector3 &vec1, const CxVector3 &vec2, const CxVector3 &point);
 
225
 
 
226
        void Dump() const;
 
227
 
 
228
private:
 
229
        float m_pData[3];
 
230
};
 
231
 
 
232
 
 
233
float Dihedral(const CxVector3 &vec1, const CxVector3 &vec2, const CxVector3 &norm, bool absolute);
 
234
 
 
235
float Dihedral2(const CxVector3 &vec1, const CxVector3 &vec2, const CxVector3 &norm, bool absolute);
 
236
 
 
237
 
 
238
inline void Swap(CxVector3 &vec1, CxVector3 &vec2)
 
239
{
 
240
        BXIN;
 
241
        CxVector3 t;
 
242
        t = vec1;
 
243
        vec1 = vec2;
 
244
        vec2 = t;
 
245
        BXOUT;
 
246
}
 
247
 
 
248
 
 
249
inline CxVector3 operator * (const float &f, const CxVector3 &v)
 
250
{
 
251
        return v*f;
 
252
}
 
253
 
 
254
 
 
255
inline CxVector3 CrossP(const CxVector3 &vec1, const CxVector3 &vec2)
 
256
{
 
257
        return CxVector3(vec1[1]*vec2[2] - vec1[2]*vec2[1],
 
258
                vec1[2]*vec2[0] - vec1[0]*vec2[2],
 
259
                vec1[0]*vec2[1] - vec1[1]*vec2[0]);
 
260
}
 
261
 
 
262
 
 
263
inline float DotP(const CxVector3 &vec1, const CxVector3 &vec2)
 
264
{
 
265
        return vec1[0]*vec2[0]+vec1[1]*vec2[1]+vec1[2]*vec2[2];
 
266
}
 
267
 
 
268
 
 
269
inline float Angle(const CxVector3 &vec1, const CxVector3 &vec2)
 
270
{
 
271
        BXIN;
 
272
        float t;
 
273
        t = DotP(vec1,vec2) / vec1.GetLength() / vec2.GetLength();
 
274
        if (t > 1.0f)
 
275
                t = 1.0f;
 
276
        if (t < -1.0f)
 
277
                t = -1.0f;
 
278
        return (float)acos(t);
 
279
        BXOUT;
 
280
}
 
281
 
 
282
 
 
283
inline float Angle_Deg(const CxVector3 &vec1, const CxVector3 &vec2)
 
284
{
 
285
        BXIN;
 
286
        float t;
 
287
        t = DotP(vec1,vec2) / vec1.GetLength() / vec2.GetLength();
 
288
        if (t > 1.0f)
 
289
                t = 1.0f;
 
290
        if (t < -1.0f)
 
291
                t = -1.0f;
 
292
        t = (float)acos(t);
 
293
        BXOUT;
 
294
        return (float)fabs(t*180.0f / Pi);
 
295
}
 
296
 
 
297
 
 
298
inline float VecDist(const CxVector3 &vec1, const CxVector3 &vec2)
 
299
{
 
300
        return (float)sqrt((vec1[0]-vec2[0])*(vec1[0]-vec2[0]) + (vec1[1]-vec2[1])*(vec1[1]-vec2[1]) + (vec1[2]-vec2[2])*(vec1[2]-vec2[2]));
 
301
}
 
302
 
 
303
 
 
304
CxVector3 PointFromRAD(CxVector3 r1, CxVector3 r2, CxVector3 r3, float r, float a, float d);
 
305
 
 
306
#endif