mirror of https://github.com/ArduPilot/ardupilot
AP_Math: const correctness
This commit is contained in:
Pierre Kancir
Randy Mackay
parent
f1aa4f3f87
commit
f1270b4b22
|
|
@ -25,12 +25,12 @@
|
|||
template <typename T>
|
||||
void Matrix3<T>::from_euler(float roll, float pitch, float yaw)
|
||||
{
|
||||
float cp = cosf(pitch);
|
||||
float sp = sinf(pitch);
|
||||
float sr = sinf(roll);
|
||||
float cr = cosf(roll);
|
||||
float sy = sinf(yaw);
|
||||
float cy = cosf(yaw);
|
||||
const float cp = cosf(pitch);
|
||||
const float sp = sinf(pitch);
|
||||
const float sr = sinf(roll);
|
||||
const float cr = cosf(roll);
|
||||
const float sy = sinf(yaw);
|
||||
const float cy = cosf(yaw);
|
||||
|
||||
a.x = cp * cy;
|
||||
a.y = (sr * sp * cy) - (cr * sy);
|
||||
|
|
@ -91,12 +91,12 @@ Vector3<T> Matrix3<T>::to_euler312() const
|
|||
template <typename T>
|
||||
void Matrix3<T>::from_euler312(float roll, float pitch, float yaw)
|
||||
{
|
||||
float c3 = cosf(pitch);
|
||||
float s3 = sinf(pitch);
|
||||
float s2 = sinf(roll);
|
||||
float c2 = cosf(roll);
|
||||
float s1 = sinf(yaw);
|
||||
float c1 = cosf(yaw);
|
||||
const float c3 = cosf(pitch);
|
||||
const float s3 = sinf(pitch);
|
||||
const float s2 = sinf(roll);
|
||||
const float c2 = cosf(roll);
|
||||
const float s1 = sinf(yaw);
|
||||
const float c1 = cosf(yaw);
|
||||
|
||||
a.x = c1 * c3 - s1 * s2 * s3;
|
||||
b.y = c1 * c2;
|
||||
|
|
@ -134,10 +134,10 @@ void Matrix3<T>::rotate(const Vector3<T> &g)
|
|||
template <typename T>
|
||||
void Matrix3<T>::normalize(void)
|
||||
{
|
||||
float error = a * b;
|
||||
Vector3<T> t0 = a - (b * (0.5f * error));
|
||||
Vector3<T> t1 = b - (a * (0.5f * error));
|
||||
Vector3<T> t2 = t0 % t1;
|
||||
const float error = a * b;
|
||||
const Vector3<T> t0 = a - (b * (0.5f * error));
|
||||
const Vector3<T> t1 = b - (a * (0.5f * error));
|
||||
const Vector3<T> t2 = t0 % t1;
|
||||
a = t0 * (1.0f / t0.length());
|
||||
b = t1 * (1.0f / t1.length());
|
||||
c = t2 * (1.0f / t2.length());
|
||||
|
|
@ -204,7 +204,7 @@ T Matrix3<T>::det() const
|
|||
template <typename T>
|
||||
bool Matrix3<T>::inverse(Matrix3<T>& inv) const
|
||||
{
|
||||
T d = det();
|
||||
const T d = det();
|
||||
|
||||
if (is_zero(d)) {
|
||||
return false;
|
||||
|
|
@ -247,14 +247,14 @@ void Matrix3<T>::zero(void)
|
|||
template <typename T>
|
||||
void Matrix3<T>::from_axis_angle(const Vector3<T> &v, float theta)
|
||||
{
|
||||
float C = cosf(theta);
|
||||
float S = sinf(theta);
|
||||
float t = 1.0f - C;
|
||||
Vector3f normv = v.normalized();
|
||||
float x = normv.x;
|
||||
float y = normv.y;
|
||||
float z = normv.z;
|
||||
|
||||
const float C = cosf(theta);
|
||||
const float S = sinf(theta);
|
||||
const float t = 1.0f - C;
|
||||
const Vector3f normv = v.normalized();
|
||||
const float x = normv.x;
|
||||
const float y = normv.y;
|
||||
const float z = normv.z;
|
||||
|
||||
a.x = t*x*x + C;
|
||||
a.y = t*x*y - z*S;
|
||||
a.z = t*x*z + y*S;
|
||||
|
|
|
|||
|
|
@ -23,15 +23,15 @@
|
|||
// return the rotation matrix equivalent for this quaternion
|
||||
void Quaternion::rotation_matrix(Matrix3f &m) const
|
||||
{
|
||||
float q3q3 = q3 * q3;
|
||||
float q3q4 = q3 * q4;
|
||||
float q2q2 = q2 * q2;
|
||||
float q2q3 = q2 * q3;
|
||||
float q2q4 = q2 * q4;
|
||||
float q1q2 = q1 * q2;
|
||||
float q1q3 = q1 * q3;
|
||||
float q1q4 = q1 * q4;
|
||||
float q4q4 = q4 * q4;
|
||||
const float q3q3 = q3 * q3;
|
||||
const float q3q4 = q3 * q4;
|
||||
const float q2q2 = q2 * q2;
|
||||
const float q2q3 = q2 * q3;
|
||||
const float q2q4 = q2 * q4;
|
||||
const float q1q2 = q1 * q2;
|
||||
const float q1q3 = q1 * q3;
|
||||
const float q1q4 = q1 * q4;
|
||||
const float q4q4 = q4 * q4;
|
||||
|
||||
m.a.x = 1.0f-2.0f*(q3q3 + q4q4);
|
||||
m.a.y = 2.0f*(q2q3 - q1q4);
|
||||
|
|
@ -47,17 +47,17 @@ void Quaternion::rotation_matrix(Matrix3f &m) const
|
|||
// return the rotation matrix equivalent for this quaternion after normalization
|
||||
void Quaternion::rotation_matrix_norm(Matrix3f &m) const
|
||||
{
|
||||
float q1q1 = q1 * q1;
|
||||
float q1q2 = q1 * q2;
|
||||
float q1q3 = q1 * q3;
|
||||
float q1q4 = q1 * q4;
|
||||
float q2q2 = q2 * q2;
|
||||
float q2q3 = q2 * q3;
|
||||
float q2q4 = q2 * q4;
|
||||
float q3q3 = q3 * q3;
|
||||
float q3q4 = q3 * q4;
|
||||
float q4q4 = q4 * q4;
|
||||
float invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
|
||||
const float q1q1 = q1 * q1;
|
||||
const float q1q2 = q1 * q2;
|
||||
const float q1q3 = q1 * q3;
|
||||
const float q1q4 = q1 * q4;
|
||||
const float q2q2 = q2 * q2;
|
||||
const float q2q3 = q2 * q3;
|
||||
const float q2q4 = q2 * q4;
|
||||
const float q3q3 = q3 * q3;
|
||||
const float q3q4 = q3 * q4;
|
||||
const float q4q4 = q4 * q4;
|
||||
const float invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
|
||||
|
||||
m.a.x = ( q2q2 - q3q3 - q4q4 + q1q1)*invs;
|
||||
m.a.y = 2.0f*(q2q3 - q1q4)*invs;
|
||||
|
|
@ -89,28 +89,28 @@ void Quaternion::from_rotation_matrix(const Matrix3f &m)
|
|||
float &qy = q3;
|
||||
float &qz = q4;
|
||||
|
||||
float tr = m00 + m11 + m22;
|
||||
const float tr = m00 + m11 + m22;
|
||||
|
||||
if (tr > 0) {
|
||||
float S = sqrtf(tr+1) * 2;
|
||||
const float S = sqrtf(tr+1) * 2;
|
||||
qw = 0.25f * S;
|
||||
qx = (m21 - m12) / S;
|
||||
qy = (m02 - m20) / S;
|
||||
qz = (m10 - m01) / S;
|
||||
} else if ((m00 > m11) && (m00 > m22)) {
|
||||
float S = sqrtf(1.0f + m00 - m11 - m22) * 2.0f;
|
||||
const float S = sqrtf(1.0f + m00 - m11 - m22) * 2.0f;
|
||||
qw = (m21 - m12) / S;
|
||||
qx = 0.25f * S;
|
||||
qy = (m01 + m10) / S;
|
||||
qz = (m02 + m20) / S;
|
||||
} else if (m11 > m22) {
|
||||
float S = sqrtf(1.0f + m11 - m00 - m22) * 2.0f;
|
||||
const float S = sqrtf(1.0f + m11 - m00 - m22) * 2.0f;
|
||||
qw = (m02 - m20) / S;
|
||||
qx = (m01 + m10) / S;
|
||||
qy = 0.25f * S;
|
||||
qz = (m12 + m21) / S;
|
||||
} else {
|
||||
float S = sqrtf(1.0f + m22 - m00 - m11) * 2.0f;
|
||||
const float S = sqrtf(1.0f + m22 - m00 - m11) * 2.0f;
|
||||
qw = (m10 - m01) / S;
|
||||
qx = (m02 + m20) / S;
|
||||
qy = (m12 + m21) / S;
|
||||
|
|
@ -129,12 +129,12 @@ void Quaternion::earth_to_body(Vector3f &v) const
|
|||
// create a quaternion from Euler angles
|
||||
void Quaternion::from_euler(float roll, float pitch, float yaw)
|
||||
{
|
||||
float cr2 = cosf(roll*0.5f);
|
||||
float cp2 = cosf(pitch*0.5f);
|
||||
float cy2 = cosf(yaw*0.5f);
|
||||
float sr2 = sinf(roll*0.5f);
|
||||
float sp2 = sinf(pitch*0.5f);
|
||||
float sy2 = sinf(yaw*0.5f);
|
||||
const float cr2 = cosf(roll*0.5f);
|
||||
const float cp2 = cosf(pitch*0.5f);
|
||||
const float cy2 = cosf(yaw*0.5f);
|
||||
const float sr2 = sinf(roll*0.5f);
|
||||
const float sp2 = sinf(pitch*0.5f);
|
||||
const float sy2 = sinf(yaw*0.5f);
|
||||
|
||||
q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
|
||||
q2 = sr2*cp2*cy2 - cr2*sp2*sy2;
|
||||
|
|
@ -153,7 +153,7 @@ void Quaternion::from_vector312(float roll ,float pitch, float yaw)
|
|||
|
||||
void Quaternion::from_axis_angle(Vector3f v)
|
||||
{
|
||||
float theta = v.length();
|
||||
const float theta = v.length();
|
||||
if (is_zero(theta)) {
|
||||
q1 = 1.0f;
|
||||
q2=q3=q4=0.0f;
|
||||
|
|
@ -171,7 +171,7 @@ void Quaternion::from_axis_angle(const Vector3f &axis, float theta)
|
|||
q2=q3=q4=0.0f;
|
||||
return;
|
||||
}
|
||||
float st2 = sinf(theta/2.0f);
|
||||
const float st2 = sinf(theta/2.0f);
|
||||
|
||||
q1 = cosf(theta/2.0f);
|
||||
q2 = axis.x * st2;
|
||||
|
|
@ -188,7 +188,7 @@ void Quaternion::rotate(const Vector3f &v)
|
|||
|
||||
void Quaternion::to_axis_angle(Vector3f &v)
|
||||
{
|
||||
float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
|
||||
const float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
|
||||
v = Vector3f(q2,q3,q4);
|
||||
if (!is_zero(l)) {
|
||||
v /= l;
|
||||
|
|
@ -198,7 +198,7 @@ void Quaternion::to_axis_angle(Vector3f &v)
|
|||
|
||||
void Quaternion::from_axis_angle_fast(Vector3f v)
|
||||
{
|
||||
float theta = v.length();
|
||||
const float theta = v.length();
|
||||
if (is_zero(theta)) {
|
||||
q1 = 1.0f;
|
||||
q2=q3=q4=0.0f;
|
||||
|
|
@ -210,9 +210,9 @@ void Quaternion::from_axis_angle_fast(Vector3f v)
|
|||
|
||||
void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta)
|
||||
{
|
||||
float t2 = theta/2.0f;
|
||||
float sqt2 = sq(t2);
|
||||
float st2 = t2-sqt2*t2/6.0f;
|
||||
const float t2 = theta/2.0f;
|
||||
const float sqt2 = sq(t2);
|
||||
const float st2 = t2-sqt2*t2/6.0f;
|
||||
|
||||
q1 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
|
||||
q2 = axis.x * st2;
|
||||
|
|
@ -222,26 +222,26 @@ void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta)
|
|||
|
||||
void Quaternion::rotate_fast(const Vector3f &v)
|
||||
{
|
||||
float theta = v.length();
|
||||
const float theta = v.length();
|
||||
if (is_zero(theta)) {
|
||||
return;
|
||||
}
|
||||
float t2 = theta/2.0f;
|
||||
float sqt2 = sq(t2);
|
||||
const float t2 = theta/2.0f;
|
||||
const float sqt2 = sq(t2);
|
||||
float st2 = t2-sqt2*t2/6.0f;
|
||||
st2 /= theta;
|
||||
|
||||
//"rotation quaternion"
|
||||
float w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
|
||||
float x2 = v.x * st2;
|
||||
float y2 = v.y * st2;
|
||||
float z2 = v.z * st2;
|
||||
const float w2 = 1.0f-(sqt2/2.0f)+sq(sqt2)/24.0f;
|
||||
const float x2 = v.x * st2;
|
||||
const float y2 = v.y * st2;
|
||||
const float z2 = v.z * st2;
|
||||
|
||||
//copy our quaternion
|
||||
float w1 = q1;
|
||||
float x1 = q2;
|
||||
float y1 = q3;
|
||||
float z1 = q4;
|
||||
const float w1 = q1;
|
||||
const float x1 = q2;
|
||||
const float y1 = q3;
|
||||
const float z1 = q4;
|
||||
|
||||
//do the multiply into our quaternion
|
||||
q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
|
||||
|
|
@ -296,9 +296,9 @@ Quaternion Quaternion::inverse(void) const
|
|||
|
||||
void Quaternion::normalize(void)
|
||||
{
|
||||
float quatMag = length();
|
||||
const float quatMag = length();
|
||||
if (!is_zero(quatMag)) {
|
||||
float quatMagInv = 1.0f/quatMag;
|
||||
const float quatMagInv = 1.0f/quatMag;
|
||||
q1 *= quatMagInv;
|
||||
q2 *= quatMagInv;
|
||||
q3 *= quatMagInv;
|
||||
|
|
@ -314,10 +314,10 @@ Quaternion Quaternion::operator*(const Quaternion &v) const
|
|||
const float &y1 = q3;
|
||||
const float &z1 = q4;
|
||||
|
||||
float w2 = v.q1;
|
||||
float x2 = v.q2;
|
||||
float y2 = v.q3;
|
||||
float z2 = v.q4;
|
||||
const float w2 = v.q1;
|
||||
const float x2 = v.q2;
|
||||
const float y2 = v.q3;
|
||||
const float z2 = v.q4;
|
||||
|
||||
ret.q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
|
||||
ret.q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
|
||||
|
|
@ -329,15 +329,15 @@ Quaternion Quaternion::operator*(const Quaternion &v) const
|
|||
|
||||
Quaternion &Quaternion::operator*=(const Quaternion &v)
|
||||
{
|
||||
float w1 = q1;
|
||||
float x1 = q2;
|
||||
float y1 = q3;
|
||||
float z1 = q4;
|
||||
const float w1 = q1;
|
||||
const float x1 = q2;
|
||||
const float y1 = q3;
|
||||
const float z1 = q4;
|
||||
|
||||
float w2 = v.q1;
|
||||
float x2 = v.q2;
|
||||
float y2 = v.q3;
|
||||
float z2 = v.q4;
|
||||
const float w2 = v.q1;
|
||||
const float x2 = v.q2;
|
||||
const float y2 = v.q3;
|
||||
const float z2 = v.q4;
|
||||
|
||||
q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
|
||||
q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
|
||||
|
|
@ -355,10 +355,10 @@ Quaternion Quaternion::operator/(const Quaternion &v) const
|
|||
const float &quat2 = q3;
|
||||
const float &quat3 = q4;
|
||||
|
||||
float rquat0 = v.q1;
|
||||
float rquat1 = v.q2;
|
||||
float rquat2 = v.q3;
|
||||
float rquat3 = v.q4;
|
||||
const float rquat0 = v.q1;
|
||||
const float rquat1 = v.q2;
|
||||
const float rquat2 = v.q3;
|
||||
const float rquat3 = v.q4;
|
||||
|
||||
ret.q1 = (rquat0*quat0 + rquat1*quat1 + rquat2*quat2 + rquat3*quat3);
|
||||
ret.q2 = (rquat0*quat1 - rquat1*quat0 - rquat2*quat3 + rquat3*quat2);
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@ void splinterp5(const float x[5], float out[4][4])
|
|||
} else if (p > -0.01f && p < 0.0f) {
|
||||
p = -0.01f;
|
||||
}
|
||||
float p_inv = 1.0f / p;
|
||||
const float p_inv = 1.0f / p;
|
||||
z[i] = -0.5f * p_inv;
|
||||
u[i] = x[i+1] + x[i-1] - 2.0f * x[i];
|
||||
u[i] = (3.0f * u[i] - 0.5f * u[i-1]) * p_inv;
|
||||
|
|
|
|||
|
|
@ -126,11 +126,11 @@ bool Vector2<T>::operator !=(const Vector2<T> &v) const
|
|||
template <typename T>
|
||||
float Vector2<T>::angle(const Vector2<T> &v2) const
|
||||
{
|
||||
float len = this->length() * v2.length();
|
||||
const float len = this->length() * v2.length();
|
||||
if (len <= 0) {
|
||||
return 0.0f;
|
||||
}
|
||||
float cosv = ((*this)*v2) / len;
|
||||
const float cosv = ((*this)*v2) / len;
|
||||
if (cosv >= 1) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
|
@ -158,8 +158,8 @@ bool Vector2<T>::segment_intersection(const Vector2<T>& seg1_start, const Vector
|
|||
} else {
|
||||
// t = (q - p) * s / (r * s)
|
||||
// u = (q - p) * r / (r * s)
|
||||
float t = (ss2_ss1 % r2) / r1xr2;
|
||||
float u = q_pxr / r1xr2;
|
||||
const float t = (ss2_ss1 % r2) / r1xr2;
|
||||
const float u = q_pxr / r1xr2;
|
||||
if ((u >= 0) && (u <= 1) && (t >= 0) && (t <= 1)) {
|
||||
// lines intersect
|
||||
// t can be any non-negative value because (p, p + r) is a ray
|
||||
|
|
|
|||
|
|
@ -154,7 +154,7 @@ struct Vector2
|
|||
// reflects this vector about n
|
||||
void reflect(const Vector2<T> &n)
|
||||
{
|
||||
Vector2<T> orig(*this);
|
||||
const Vector2<T> orig(*this);
|
||||
project(n);
|
||||
*this= *this*2 - orig;
|
||||
}
|
||||
|
|
@ -175,10 +175,10 @@ struct Vector2
|
|||
// perpendicular to v1 maximising distance from p1
|
||||
static Vector2<T> perpendicular(const Vector2<T> &pos_delta, const Vector2<T> &v1)
|
||||
{
|
||||
Vector2<T> perpendicular1 = Vector2<T>(-v1[1], v1[0]);
|
||||
Vector2<T> perpendicular2 = Vector2<T>(v1[1], -v1[0]);
|
||||
T d1 = perpendicular1 * pos_delta;
|
||||
T d2 = perpendicular2 * pos_delta;
|
||||
const Vector2<T> perpendicular1 = Vector2<T>(-v1[1], v1[0]);
|
||||
const Vector2<T> perpendicular2 = Vector2<T>(v1[1], -v1[0]);
|
||||
const T d1 = perpendicular1 * pos_delta;
|
||||
const T d2 = perpendicular2 * pos_delta;
|
||||
if (d1 > d2) {
|
||||
return perpendicular1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -375,11 +375,11 @@ bool Vector3<T>::operator !=(const Vector3<T> &v) const
|
|||
template <typename T>
|
||||
float Vector3<T>::angle(const Vector3<T> &v2) const
|
||||
{
|
||||
float len = this->length() * v2.length();
|
||||
const float len = this->length() * v2.length();
|
||||
if (len <= 0) {
|
||||
return 0.0f;
|
||||
}
|
||||
float cosv = ((*this)*v2) / len;
|
||||
const float cosv = ((*this)*v2) / len;
|
||||
if (fabsf(cosv) >= 1) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
|
@ -410,9 +410,9 @@ template <typename T>
|
|||
float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const
|
||||
{
|
||||
// triangle side lengths
|
||||
float a = (*this-seg_start).length();
|
||||
float b = (seg_start-seg_end).length();
|
||||
float c = (seg_end-*this).length();
|
||||
const float a = (*this-seg_start).length();
|
||||
const float b = (seg_start-seg_end).length();
|
||||
const float c = (seg_end-*this).length();
|
||||
|
||||
// protect against divide by zero later
|
||||
if (::is_zero(b)) {
|
||||
|
|
@ -420,14 +420,14 @@ float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3
|
|||
}
|
||||
|
||||
// semiperimeter of triangle
|
||||
float s = (a+b+c) * 0.5f;
|
||||
const float s = (a+b+c) * 0.5f;
|
||||
|
||||
float area_squared = s*(s-a)*(s-b)*(s-c);
|
||||
// area must be constrained above 0 because a triangle could have 3 points could be on a line and float rounding could push this under 0
|
||||
if (area_squared < 0.0f) {
|
||||
area_squared = 0.0f;
|
||||
}
|
||||
float area = safe_sqrt(area_squared);
|
||||
const float area = safe_sqrt(area_squared);
|
||||
return 2.0f*area/b;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -218,9 +218,9 @@ public:
|
|||
|
||||
// distance from the tip of this vector to another vector squared (so as to avoid the sqrt calculation)
|
||||
float distance_squared(const Vector3<T> &v) const {
|
||||
float dist_x = x-v.x;
|
||||
float dist_y = y-v.y;
|
||||
float dist_z = z-v.z;
|
||||
const float dist_x = x-v.x;
|
||||
const float dist_y = y-v.y;
|
||||
const float dist_z = z-v.z;
|
||||
return (dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
|
||||
}
|
||||
|
||||
|
|
@ -233,11 +233,11 @@ public:
|
|||
// zero vector - that should be checked for.
|
||||
static Vector3<T> perpendicular(const Vector3<T> &p1, const Vector3<T> &v1)
|
||||
{
|
||||
T d = p1 * v1;
|
||||
const T d = p1 * v1;
|
||||
if (fabsf(d) < FLT_EPSILON) {
|
||||
return p1;
|
||||
}
|
||||
Vector3<T> parallel = (v1 * d) / v1.length_squared();
|
||||
const Vector3<T> parallel = (v1 * d) / v1.length_squared();
|
||||
Vector3<T> perpendicular = p1 - parallel;
|
||||
|
||||
return perpendicular;
|
||||
|
|
|
|||
Loading…
Reference in New Issue