1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
|
/*
*
* SIXENSE CONFIDENTIAL
*
* Copyright (C) 2011 Sixense Entertainment Inc.
* All Rights Reserved
*
*/
#ifndef SIXENSE_MATH_HPP
#define SIXENSE_MATH_HPP
#include <math.h>
#include <iostream>
#include <string>
namespace sixenseMath {
class Vector2 {
public:
Vector2();
Vector2( const float x, const float y );
float& operator [](const int idx);
Vector2 operator -(const Vector2 rhs);
Vector2 operator +(const Vector2 rhs);
Vector2& operator +=(const Vector2& rhs);
Vector2& operator *=(const float& rhs);
Vector2& operator -=(const Vector2& rhs);
float operator *(const Vector2 rhs); // dot product
Vector2 operator *(const float rhs);
Vector2 operator /(const float rhs);
bool operator ==(const Vector2& rhs);
void normalize();
float length();
void print( const std::string name=std::string() );
void fill( float vec[2] );
protected:
float _vec[2];
};
class Vector3 {
public:
Vector3();
Vector3( const Vector3& );
Vector3( const float vec[3] );
Vector3( const float x, const float y, const float z );
float& operator [](const int idx);
Vector3 operator -(const Vector3 rhs);
Vector3 operator +(const Vector3 rhs);
Vector3 operator ^(const Vector3 rhs); // cross product
Vector3& operator +=(const Vector3& rhs);
Vector3& operator *=(const float& rhs);
Vector3& operator -=(const Vector3& rhs);
float operator *(const Vector3 rhs); // dot product
Vector3 operator *(const float rhs);
Vector3 operator /(const float rhs);
void normalize();
float length();
void print( const std::string name=std::string() );
void fill( float vec[3] );
bool operator ==(const Vector3& rhs);
// Construction helpers
static Vector3 normalize( const Vector3 );
protected:
float _vec[3];
};
class Vector4 {
public:
Vector4();
Vector4( const Vector4& );
Vector4( const Vector3&, float w );
Vector4( const float vec[4] );
Vector4( const float x, const float y, const float z, const float w );
float& operator [](const int idx);
Vector4 operator -(const Vector4 rhs) const;
Vector4 operator ^(const Vector4 rhs) const; // cross product
Vector4 operator +(const Vector4 rhs) const;
float operator *(const Vector4 rhs); // dot product
Vector4 operator *(const float rhs) const;
Vector4 operator -(const float rhs) const;
Vector4 operator /(const float rhs) const;
bool operator ==(const Vector4& rhs);
void normalize();
float length();
void print( const std::string name=std::string() );
Vector4 operator *(const class Matrix4 rhs) const;
void fill( float vec[4] );
protected:
float _vec[4];
};
class Quat : public Vector4 {
public:
Quat();
Quat( const float x, const float y, const float z, const float w );
Quat( const class Matrix3& );
Quat( const Vector4& );
Quat( const Vector3 xyz, float w );
void print( const std::string name=std::string() );
Vector3 operator *(const Vector3 rhs) const;
Quat operator *(const Quat rhs) const;
float dot(const Quat rhs) const; // dot product (operator * is already used for quat...)
void invert(); // invert in place
Quat inverse(); // leave this alone and return inverted copy
Vector3 getEulerAngles();
// Construction helpers
static Quat rotation( const Vector3 from_vec, const Vector3 to_vec );
static Quat rotation( const float angle_in_rad, const Vector3 axis );
static Quat rotation( const Vector3 hpr_in_rad );
static Quat rotation( const float heading, const float pitch, const float roll );
static Quat slerp( const float t, const Quat a, const Quat b);
};
class Matrix3 {
public:
Matrix3();
Matrix3( const Matrix3& );
Matrix3( const float mat[3][3] );
Matrix3( const float m00, const float m10, const float m20, const float m01, const float m11, const float m21, const float m02, const float m12, const float m22 );
void fill( float mat[3][3] );
Matrix3( const Vector3 col0, const Vector3 col1, const Vector3 col2 );
Vector3& operator [](const int idx);
Matrix3 operator *(const Matrix3 rhs);
Matrix3 operator *(const float rhs);
Matrix3 operator /(const float rhs);
Matrix3 operator +(const Matrix3 rhs);
Matrix3 operator -(const Matrix3 rhs);
Matrix3 operator *(const Quat rhs);
Vector3 operator *(const Vector3 rhs);
Vector3 col( const int );
Vector3 row( const int );
void set_col( const int which, const Vector3 col );
float trace();
bool is_identity();
void transpose();
void print( const std::string name=std::string() );
Vector3 getEulerAngles();
// Construction helpers
static Matrix3 rotation( const float angle_in_rad, const Vector3 axis );
static Matrix3 rotation( const Vector3 hpr_in_rad );
static Matrix3 rotation( const Quat rot );
static Matrix3 rotation( const Vector3 from, const Vector3 to );
static Matrix3 translation( const Vector3 trans );
static Matrix3 scale( const float, const float, const float );
static Matrix3 scale( const float );
static Matrix3 transpose( const Matrix3 );
protected:
Vector3 _cols[3];
};
class Matrix4 {
public:
Matrix4();
Matrix4( const Matrix4& );
Matrix4( const Matrix3& );
Matrix4( const float mat[4][4] );
Matrix4( const float m00, const float m10, const float m20, const float m30, const float m01, const float m11, const float m21, const float m31, const float m02, const float m12, const float m22, const float m32, const float m03, const float m13, const float m23, const float m33 );
void fill( float mat[4][4] );
Matrix4( const Vector4 col0, const Vector4 col1, const Vector4 col2, const Vector4 col3 );
Vector4& operator [](const int idx);
Matrix4 operator *(const Matrix4 rhs);
Matrix4 operator *(const float rhs);
Matrix4 operator /(const float rhs);
Matrix4 operator +(const Matrix4 rhs);
Matrix4 operator -(const Matrix4 rhs);
Matrix4 operator *(const Quat rhs);
Vector4 operator *(const Vector4 rhs);
Vector4 col( const int );
Vector4 row( const int );
void set_col( const int which, const Vector4 col );
float trace();
bool is_identity();
void transpose();
void print( const std::string name=std::string() );
Vector3 getEulerAngles();
// Construction helpers
static Matrix4 rotation( const float angle_in_rad, const Vector3 axis );
static Matrix4 rotation( const Quat rot );
static Matrix4 rotation( const Vector3 from, const Vector3 to );
static Matrix4 rotation( const Vector3 hpr_in_rad );
static Matrix4 translation( const Vector3 trans );
static Matrix4 scale( const float, const float, const float );
static Matrix4 scale( const float );
static Matrix4 transpose( const Matrix4 );
protected:
Vector4 _cols[4];
};
class Line {
friend class Plane;
public:
Line( const Line& );
Line( const Vector3& dir, const Vector3& pos );
Vector3 getClosestPoint( const Vector3& );
private:
Vector3 _dir;
Vector3 _pos1;
Vector3 _pos2;
};
class Plane {
public:
Plane();
Plane( const Plane& );
Plane( Vector3 p0, Vector3 p1, Vector3 p2 );
Plane( Vector3 point, Vector3 normal );
void init();
double whichSide( Vector3 p );
Vector3 getClosestPoint( Vector3 in );
Vector3 intersect( const Line line );
Vector3 getNormal();
private:
double _a, _b, _c, _d;
Vector3 _norm;
Vector3 _p0, _p1, _p2;
};
#include "sixense_math.cpp"
}
#endif
|
