| class Quaternion { | |
| double data[4]; | |
| public: | |
| Quaternion() { memset(data, 0, 4*sizeof(double)); } | |
| Quaternion(const double s) { memset(data, 0, 4*sizeof(double)); data[0] = s; } | |
| Quaternion(const double s, const double x, const double y, const double z) { | |
| data[0] = s; data[1] = x; data[2] = y; data[3] = z; | |
| } | |
| Quaternion(const Quaternion& q) { memcpy(data, q.data, 4*sizeof(double)); } | |
| Quaternion operator=(const Quaternion q) { memcpy(data, q.data, 4*sizeof(double)); return *this; } | |
| Quaternion operator-() { | |
| for ( int i=0; i<4; i++ ) data[i] = -data[i]; | |
| return *this; | |
| } | |
| Quaternion operator+=(const Quaternion& q) { | |
| for ( int i=0; i<4; i++ ) data[i] += q.data[i]; | |
| return *this; | |
| } | |
| Quaternion operator+(const Quaternion& q) { | |
| Quaternion qn(*this); | |
| return qn += q; | |
| } | |
| Quaternion operator-=(const Quaternion& q) { | |
| for ( int i=0; i<4; i++ ) data[i] -= q.data[i]; | |
| return *this; | |
| } | |
| Quaternion operator-(const Quaternion& q) { | |
| Quaternion qn(*this); | |
| return qn -= q; | |
| } | |
| Quaternion operator*=(const Quaternion& q) { | |
| *this = *this * q; | |
| return *this; | |
| } | |
| Quaternion operator*(const Quaternion& q) { | |
| Quaternion qn; | |
| qn.data[0] = data[0]*q.data[0] - data[1]*q.data[1] - data[2]*q.data[2] - data[3]*q.data[3]; | |
| qn.data[1] = data[0]*q.data[1] + data[1]*q.data[0] + data[2]*q.data[3] - data[3]*q.data[2]; | |
| qn.data[2] = data[0]*q.data[2] + data[2]*q.data[0] + data[3]*q.data[1] - data[1]*q.data[3]; | |
| qn.data[3] = data[0]*q.data[3] + data[3]*q.data[0] + data[1]*q.data[2] - data[2]*q.data[1]; | |
| return qn; | |
| } | |
| Quaternion operator/=(const double d) { | |
| double div = 1/d; | |
| for ( int i=0; i<4; i++ ) data[i] *= div; | |
| return *this; | |
| } | |
| Quaternion operator/(const double d) { | |
| Quaternion q(*this); | |
| q /= d; | |
| return q; | |
| } | |
| double& operator[](int i) { if ( i<0 ) i=0; if ( i>3 ) i=3; return data[i]; } | |
| double s() { return data[0]; } | |
| double x() { return data[1]; } | |
| double y() { return data[2]; } | |
| double z() { return data[3]; } | |
| void set_s(const double d) { data[0] = d; } | |
| void set_x(const double d) { data[1] = d; } | |
| void set_y(const double d) { data[2] = d; } | |
| void set_z(const double d) { data[3] = d; } | |
| void check() { for ( int i=0; i<4; i++ ) if ( fabs(data[i]) < TRIGPRECISION ) data[i] = 0; } | |
| double norm() { return sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2] + data[3]*data[3]); } | |
| double normalize() { | |
| double size = norm(); | |
| *this /= size; | |
| return size; | |
| } | |
| Quaternion conj() { return Quaternion(data[0], -data[1], -data[2], -data[3]); } | |
| double invert() { | |
| double size2 = data[0]*data[0] + data[1]*data[1] + data[2]*data[2] + data[3]*data[3]; | |
| double div = 1/size2; | |
| data[0] *= div; | |
| data[1] *= -div; | |
| data[2] *= -div; | |
| data[3] *= -div; | |
| return sqrt(size2); | |
| } | |
| Quaternion inverse() { | |
| Quaternion q(*this); | |
| q.invert(); | |
| return q; | |
| } | |
| Quaternion rotate(const Quaternion& point) { | |
| Quaternion q(*this); | |
| q.invert(); | |
| return *this * point * q; | |
| } | |
| } ; |