Revision 0695bf91
utility/quaternion.h | ||
---|---|---|
76 | 76 |
return _z; |
77 | 77 |
} |
78 | 78 |
|
79 |
double magnitude() |
|
79 |
double w() const |
|
80 |
{ |
|
81 |
return _w; |
|
82 |
} |
|
83 |
double x() const |
|
84 |
{ |
|
85 |
return _x; |
|
86 |
} |
|
87 |
double y() const |
|
88 |
{ |
|
89 |
return _y; |
|
90 |
} |
|
91 |
double z() const |
|
92 |
{ |
|
93 |
return _z; |
|
94 |
} |
|
95 |
|
|
96 |
double magnitude() const |
|
80 | 97 |
{ |
81 | 98 |
double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z); |
82 | 99 |
return sqrt(res); |
... | ... | |
84 | 101 |
|
85 | 102 |
void normalize() |
86 | 103 |
{ |
87 |
double mag = magnitude();
|
|
104 |
double mag = magnitude();
|
|
88 | 105 |
*this = this->scale(1/mag); |
89 | 106 |
} |
90 | 107 |
|
91 | 108 |
|
92 |
Quaternion conjugate() |
|
109 |
Quaternion conjugate() const
|
|
93 | 110 |
{ |
94 | 111 |
Quaternion q; |
95 | 112 |
q.w() = _w; |
... | ... | |
148 | 165 |
} |
149 | 166 |
} |
150 | 167 |
|
151 |
void toAxisAngle(Vector<3>& axis, float& angle) |
|
168 |
void toAxisAngle(Vector<3>& axis, float& angle) const
|
|
152 | 169 |
{ |
153 | 170 |
float sqw = sqrt(1-_w*_w); |
154 | 171 |
if(sqw == 0) //it's a singularity and divide by zero, avoid |
... | ... | |
160 | 177 |
axis.z() = _z / sqw; |
161 | 178 |
} |
162 | 179 |
|
163 |
Matrix<3> toMatrix() |
|
180 |
Matrix<3> toMatrix() const
|
|
164 | 181 |
{ |
165 | 182 |
Matrix<3> ret; |
166 | 183 |
ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z)); |
... | ... | |
178 | 195 |
} |
179 | 196 |
|
180 | 197 |
|
181 |
Vector<3> toEuler() |
|
198 |
// Returns euler angles that represent the quaternion. Angles are |
|
199 |
// returned in rotation order and right-handed about the specified |
|
200 |
// axes: |
|
201 |
// |
|
202 |
// v[0] is applied 1st about z (ie, roll) |
|
203 |
// v[1] is applied 2nd about y (ie, pitch) |
|
204 |
// v[2] is applied 3rd about x (ie, yaw) |
|
205 |
// |
|
206 |
// Note that this means result.x() is not a rotation about x; |
|
207 |
// similarly for result.z(). |
|
208 |
// |
|
209 |
Vector<3> toEuler() const |
|
182 | 210 |
{ |
183 | 211 |
Vector<3> ret; |
184 | 212 |
double sqw = _w*_w; |
... | ... | |
193 | 221 |
return ret; |
194 | 222 |
} |
195 | 223 |
|
196 |
Vector<3> toAngularVelocity(float dt) |
|
224 |
Vector<3> toAngularVelocity(float dt) const
|
|
197 | 225 |
{ |
198 | 226 |
Vector<3> ret; |
199 | 227 |
Quaternion one(1.0, 0.0, 0.0, 0.0); |
... | ... | |
208 | 236 |
return ret; |
209 | 237 |
} |
210 | 238 |
|
211 |
Vector<3> rotateVector(Vector<2> v) |
|
239 |
Vector<3> rotateVector(Vector<2> v) const
|
|
212 | 240 |
{ |
213 | 241 |
Vector<3> ret(v.x(), v.y(), 0.0); |
214 | 242 |
return rotateVector(ret); |
215 | 243 |
} |
216 | 244 |
|
217 |
Vector<3> rotateVector(Vector<3> v) |
|
245 |
Vector<3> rotateVector(Vector<3> v) const
|
|
218 | 246 |
{ |
219 | 247 |
Vector<3> qv(this->x(), this->y(), this->z()); |
220 | 248 |
Vector<3> t; |
... | ... | |
223 | 251 |
} |
224 | 252 |
|
225 | 253 |
|
226 |
Quaternion operator * (Quaternion q) |
|
254 |
Quaternion operator * (Quaternion q) const
|
|
227 | 255 |
{ |
228 | 256 |
Quaternion ret; |
229 | 257 |
ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z)); |
... | ... | |
233 | 261 |
return ret; |
234 | 262 |
} |
235 | 263 |
|
236 |
Quaternion operator + (Quaternion q) |
|
264 |
Quaternion operator + (Quaternion q) const
|
|
237 | 265 |
{ |
238 | 266 |
Quaternion ret; |
239 | 267 |
ret._w = _w + q._w; |
... | ... | |
243 | 271 |
return ret; |
244 | 272 |
} |
245 | 273 |
|
246 |
Quaternion operator - (Quaternion q) |
|
274 |
Quaternion operator - (Quaternion q) const
|
|
247 | 275 |
{ |
248 | 276 |
Quaternion ret; |
249 | 277 |
ret._w = _w - q._w; |
... | ... | |
253 | 281 |
return ret; |
254 | 282 |
} |
255 | 283 |
|
256 |
Quaternion operator / (float scalar) |
|
284 |
Quaternion operator / (float scalar) const
|
|
257 | 285 |
{ |
258 | 286 |
Quaternion ret; |
259 | 287 |
ret._w = this->_w/scalar; |
... | ... | |
263 | 291 |
return ret; |
264 | 292 |
} |
265 | 293 |
|
266 |
Quaternion operator * (float scalar) |
|
294 |
Quaternion operator * (float scalar) const
|
|
267 | 295 |
{ |
268 | 296 |
Quaternion ret; |
269 | 297 |
ret._w = this->_w*scalar; |
... | ... | |
273 | 301 |
return ret; |
274 | 302 |
} |
275 | 303 |
|
276 |
Quaternion scale(double scalar)
|
|
277 |
{
|
|
304 |
Quaternion scale(double scalar) const
|
|
305 |
{
|
|
278 | 306 |
Quaternion ret; |
279 | 307 |
ret._w = this->_w*scalar; |
280 | 308 |
ret._x = this->_x*scalar; |
utility/vector.h | ||
---|---|---|
125 | 125 |
{ |
126 | 126 |
Vector ret; |
127 | 127 |
|
128 |
//the cross product is only valid for vectors with 3 dimensions,
|
|
129 |
//with the exception of higher dimensional stuff that is beyond the intended scope of this library
|
|
128 |
// The cross product is only valid for vectors with 3 dimensions,
|
|
129 |
// with the exception of higher dimensional stuff that is beyond the intended scope of this library
|
|
130 | 130 |
if(N != 3) |
131 | 131 |
return ret; |
132 | 132 |
|
... | ... | |
136 | 136 |
return ret; |
137 | 137 |
} |
138 | 138 |
|
139 |
Vector scale(double scalar) |
|
139 |
Vector scale(double scalar) const
|
|
140 | 140 |
{ |
141 | 141 |
Vector ret; |
142 | 142 |
for(int i = 0; i < N; i++) |
... | ... | |
144 | 144 |
return ret; |
145 | 145 |
} |
146 | 146 |
|
147 |
Vector invert() |
|
147 |
Vector invert() const
|
|
148 | 148 |
{ |
149 | 149 |
Vector ret; |
150 | 150 |
for(int i = 0; i < N; i++) |
... | ... | |
164 | 164 |
return p_vec[n]; |
165 | 165 |
} |
166 | 166 |
|
167 |
double operator [](int n) const |
|
168 |
{ |
|
169 |
return p_vec[n]; |
|
170 |
} |
|
171 |
|
|
167 | 172 |
double& operator ()(int n) |
168 | 173 |
{ |
169 | 174 |
return p_vec[n]; |
170 | 175 |
} |
171 | 176 |
|
172 |
Vector operator + (Vector v) |
|
177 |
double operator ()(int n) const |
|
178 |
{ |
|
179 |
return p_vec[n]; |
|
180 |
} |
|
181 |
|
|
182 |
Vector operator + (Vector v) const |
|
173 | 183 |
{ |
174 | 184 |
Vector ret; |
175 | 185 |
for(int i = 0; i < N; i++) |
... | ... | |
177 | 187 |
return ret; |
178 | 188 |
} |
179 | 189 |
|
180 |
Vector operator - (Vector v) |
|
190 |
Vector operator - (Vector v) const
|
|
181 | 191 |
{ |
182 | 192 |
Vector ret; |
183 | 193 |
for(int i = 0; i < N; i++) |
... | ... | |
185 | 195 |
return ret; |
186 | 196 |
} |
187 | 197 |
|
188 |
Vector operator * (double scalar) |
|
198 |
Vector operator * (double scalar) const
|
|
189 | 199 |
{ |
190 | 200 |
return scale(scalar); |
191 | 201 |
} |
192 | 202 |
|
193 |
Vector operator / (double scalar) |
|
203 |
Vector operator / (double scalar) const
|
|
194 | 204 |
{ |
195 | 205 |
Vector ret; |
196 | 206 |
for(int i = 0; i < N; i++) |
... | ... | |
213 | 223 |
double& x() { return p_vec[0]; } |
214 | 224 |
double& y() { return p_vec[1]; } |
215 | 225 |
double& z() { return p_vec[2]; } |
226 |
double x() const { return p_vec[0]; } |
|
227 |
double y() const { return p_vec[1]; } |
|
228 |
double z() const { return p_vec[2]; } |
|
216 | 229 |
|
217 | 230 |
|
218 | 231 |
private: |
Also available in: Unified diff