|
|
|
@ -19,7 +19,7 @@
@@ -19,7 +19,7 @@
|
|
|
|
|
|
|
|
|
|
#include "AP_Math.h" |
|
|
|
|
|
|
|
|
|
#define HALF_SQRT_2 0.70710678118654757 |
|
|
|
|
#define HALF_SQRT_2 0.70710678118654757f |
|
|
|
|
|
|
|
|
|
// rotate a vector by a standard rotation, attempting
|
|
|
|
|
// to use the minimum number of floating point operations
|
|
|
|
@ -32,8 +32,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -32,8 +32,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
case ROTATION_MAX: |
|
|
|
|
return; |
|
|
|
|
case ROTATION_YAW_45: { |
|
|
|
|
tmp = HALF_SQRT_2*(x - y); |
|
|
|
|
y = HALF_SQRT_2*(x + y); |
|
|
|
|
tmp = HALF_SQRT_2*(float)(x - y); |
|
|
|
|
y = HALF_SQRT_2*(float)(x + y); |
|
|
|
|
x = tmp; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -42,8 +42,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -42,8 +42,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
case ROTATION_YAW_135: { |
|
|
|
|
tmp = -HALF_SQRT_2*(x + y); |
|
|
|
|
y = HALF_SQRT_2*(x - y); |
|
|
|
|
tmp = -HALF_SQRT_2*(float)(x + y); |
|
|
|
|
y = HALF_SQRT_2*(float)(x - y); |
|
|
|
|
x = tmp; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -51,8 +51,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -51,8 +51,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
x = -x; y = -y; |
|
|
|
|
return; |
|
|
|
|
case ROTATION_YAW_225: { |
|
|
|
|
tmp = HALF_SQRT_2*(y - x); |
|
|
|
|
y = -HALF_SQRT_2*(x + y); |
|
|
|
|
tmp = HALF_SQRT_2*(float)(y - x); |
|
|
|
|
y = -HALF_SQRT_2*(float)(x + y); |
|
|
|
|
x = tmp; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -61,8 +61,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -61,8 +61,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
case ROTATION_YAW_315: { |
|
|
|
|
tmp = HALF_SQRT_2*(x + y); |
|
|
|
|
y = HALF_SQRT_2*(y - x); |
|
|
|
|
tmp = HALF_SQRT_2*(float)(x + y); |
|
|
|
|
y = HALF_SQRT_2*(float)(y - x); |
|
|
|
|
x = tmp; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -71,8 +71,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -71,8 +71,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
case ROTATION_ROLL_180_YAW_45: { |
|
|
|
|
tmp = HALF_SQRT_2*(x + y); |
|
|
|
|
y = HALF_SQRT_2*(x - y); |
|
|
|
|
tmp = HALF_SQRT_2*(float)(x + y); |
|
|
|
|
y = HALF_SQRT_2*(float)(x - y); |
|
|
|
|
x = tmp; z = -z; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -81,8 +81,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -81,8 +81,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
case ROTATION_ROLL_180_YAW_135: { |
|
|
|
|
tmp = HALF_SQRT_2*(y - x); |
|
|
|
|
y = HALF_SQRT_2*(y + x); |
|
|
|
|
tmp = HALF_SQRT_2*(float)(y - x); |
|
|
|
|
y = HALF_SQRT_2*(float)(y + x); |
|
|
|
|
x = tmp; z = -z; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -91,8 +91,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -91,8 +91,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
case ROTATION_ROLL_180_YAW_225: { |
|
|
|
|
tmp = -HALF_SQRT_2*(x + y); |
|
|
|
|
y = HALF_SQRT_2*(y - x); |
|
|
|
|
tmp = -HALF_SQRT_2*(float)(x + y); |
|
|
|
|
y = HALF_SQRT_2*(float)(y - x); |
|
|
|
|
x = tmp; z = -z; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -101,8 +101,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -101,8 +101,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
case ROTATION_ROLL_180_YAW_315: { |
|
|
|
|
tmp = HALF_SQRT_2*(x - y); |
|
|
|
|
y = -HALF_SQRT_2*(x + y); |
|
|
|
|
tmp = HALF_SQRT_2*(float)(x - y); |
|
|
|
|
y = -HALF_SQRT_2*(float)(x + y); |
|
|
|
|
x = tmp; z = -z; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -112,8 +112,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -112,8 +112,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
} |
|
|
|
|
case ROTATION_ROLL_90_YAW_45: { |
|
|
|
|
tmp = z; z = y; y = -tmp; |
|
|
|
|
tmp = HALF_SQRT_2*(x - y); |
|
|
|
|
y = HALF_SQRT_2*(x + y); |
|
|
|
|
tmp = HALF_SQRT_2*(float)(x - y); |
|
|
|
|
y = HALF_SQRT_2*(float)(x + y); |
|
|
|
|
x = tmp; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -124,8 +124,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -124,8 +124,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
} |
|
|
|
|
case ROTATION_ROLL_90_YAW_135: { |
|
|
|
|
tmp = z; z = y; y = -tmp; |
|
|
|
|
tmp = -HALF_SQRT_2*(x + y); |
|
|
|
|
y = HALF_SQRT_2*(x - y); |
|
|
|
|
tmp = -HALF_SQRT_2*(float)(x + y); |
|
|
|
|
y = HALF_SQRT_2*(float)(x - y); |
|
|
|
|
x = tmp; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -135,8 +135,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -135,8 +135,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
} |
|
|
|
|
case ROTATION_ROLL_270_YAW_45: { |
|
|
|
|
tmp = z; z = -y; y = tmp; |
|
|
|
|
tmp = HALF_SQRT_2*(x - y); |
|
|
|
|
y = HALF_SQRT_2*(x + y); |
|
|
|
|
tmp = HALF_SQRT_2*(float)(x - y); |
|
|
|
|
y = HALF_SQRT_2*(float)(x + y); |
|
|
|
|
x = tmp; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -147,8 +147,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
@@ -147,8 +147,8 @@ void Vector3<T>::rotate(enum Rotation rotation)
|
|
|
|
|
} |
|
|
|
|
case ROTATION_ROLL_270_YAW_135: { |
|
|
|
|
tmp = z; z = -y; y = tmp; |
|
|
|
|
tmp = -HALF_SQRT_2*(x + y); |
|
|
|
|
y = HALF_SQRT_2*(x - y); |
|
|
|
|
tmp = -HALF_SQRT_2*(float)(x + y); |
|
|
|
|
y = HALF_SQRT_2*(float)(x - y); |
|
|
|
|
x = tmp; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -339,7 +339,7 @@ bool Vector3<T>::operator !=(const Vector3<T> &v) const
@@ -339,7 +339,7 @@ bool Vector3<T>::operator !=(const Vector3<T> &v) const
|
|
|
|
|
template <typename T> |
|
|
|
|
float Vector3<T>::angle(const Vector3<T> &v2) const |
|
|
|
|
{ |
|
|
|
|
return acosf(((*this)*v2) / (this->length()*v2.length())); |
|
|
|
|
return acosf((*this)*v2) / (float)((this->length()*v2.length())); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// multiplication of transpose by a vector
|
|
|
|
|