Revision 0695bf91 utility/quaternion.h
| 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; |
Also available in: Unified diff