diff --git a/quaternion.h b/quaternion.h index 80dd6e3..52eabb7 100755 --- a/quaternion.h +++ b/quaternion.h @@ -20,7 +20,9 @@ namespace vb01{ Quaternion(){} inline bool operator==(Quaternion q){return w == q.w && x == q.x && y == q.y && z == q.z;} inline bool operator!=(Quaternion q){return w != q.w || x != q.x || y != q.y || z != q.z;} - inline Quaternion operator- (){return Quaternion(w, -x, -y, -z);} + inline friend Quaternion operator-(Quaternion q){return Quaternion(q.w, -q.x, -q.y, -q.z);} + inline friend Quaternion operator+(Quaternion q1, Quaternion q2){return Quaternion(q1.w + q2.w, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z);} + inline friend Quaternion operator-(Quaternion q1, Quaternion q2){return Quaternion(q1.w - q2.w, q1.x - q2.x, q1.y - q2.y, q1.z - q2.z);} inline Quaternion operator* (Quaternion q){ float a1 = this->w, a2 = q.w, b1 = this->x, b2 = q.x, @@ -36,15 +38,35 @@ namespace vb01{ Quaternion vQuat = (*this) * Quaternion(0, v.x, v.y, v.z) * recip(); return Vector3(vQuat.x, vQuat.y, vQuat.z); } + inline float getAngle(){return fabs(w) > 1 ? 0 : 2 * acos(w);} + inline Vector3 getAxis(){return x == 0 && y == 0 && z == 0 ? Vector3(1, 0, 0) : Vector3(x, y, z).norm();} + template friend Quaternion operator+(Quaternion q, T s){return Quaternion(q.w + s, q.x + s, q.y + s, q.z + s);} + template friend Quaternion operator-(Quaternion q, T s){return Quaternion(q.w - s, q.x - s, q.y - s, q.z - s);} + template friend Quaternion operator*(Quaternion q, T s){return Quaternion(q.w * s, q.x * s, q.y * s, q.z * s);} + template friend Quaternion operator/(Quaternion q, T s){return Quaternion(q.w / s, q.x / s, q.y / s, q.z / s);} + template friend Quaternion operator+(T s, Quaternion q){return Quaternion(q.w + s, q.x + s, q.y + s, q.z + s);} + template friend Quaternion operator-(T s, Quaternion q){return Quaternion(q.w - s, q.x - s, q.y - s, q.z - s);} + template friend Quaternion operator*(T s, Quaternion q){return Quaternion(q.w * s, q.x * s, q.y * s, q.z * s);} + template friend Quaternion operator/(T s, Quaternion q){return Quaternion(q.w / s, q.x / s, q.y / s, q.z / s);} + void operator+=(Quaternion q){w += q.w, x += q.x, y += q.y, z += q.z;} + void operator-=(Quaternion q){w -= q.w, x -= q.x, y -= q.y, z -= q.z;} + void operator*=(Quaternion q){ + float a1 = this->w, a2 = q.w, + b1 = this->x, b2 = q.x, + c1 = this->y, c2 = q.y, + d1 = this->z, d2 = q.z; + w = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2; + x = a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2; + y = a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2; + z = a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2; + } + template void operator+=(T s){w += s, x += s, y += s, z += s;} + template void operator-=(T s){w -= s, x -= s, y -= s, z -= s;} + template void operator*=(T s){w *= s, x *= s, y *= s, z *= s;} + template void operator/=(T s){w /= s, x /= s, y /= s, z /= s;} inline Quaternion fromAngle(float angle, Vector3 axis){ return Quaternion(cos(angle / 2), axis.x * sin(angle / 2), axis.y * sin(angle / 2), axis.z * sin(angle / 2)); } - inline float getAngle(){return fabs(w) > 1 ? 0 : 2 * acos(w);} - inline Vector3 getAxis(){return x == 0 && y == 0 && z == 0 ? Vector3(1, 0, 0) : Vector3(x, y, z).norm();} - inline Quaternion operator+(Quaternion q){return Quaternion(w + q.w, x + q.x, y + q.y, z + q.z);} - inline Quaternion operator-(Quaternion q){return Quaternion(w - q.w, x - q.x, y - q.y, z - q.z);} - template inline Quaternion operator*(T s){return Quaternion(w * s, x * s, y * s, z * s);} - template inline Quaternion operator/(T s){return Quaternion(w / s, x / s, y / s, z / s);} inline Quaternion conj(){ return (*this + Quaternion(0, 1, 0, 0) *(*this) * Quaternion(0, 1, 0, 0) + Quaternion(0, 0, 1, 0)