livie
/
phoenix-firestorm
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

merge changes for storm-1915

master
Oz Linden 2013-09-11 11:27:56 -04:00
parent 55ae6a7962 681305e9bf
commit 493a34aa15
4 changed files with 263 additions and 269 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
indra/llmath/llmath.h
View File

@ -1,4 +1,4 @@
/**
/**
* @file llmath.h
* @brief Useful math constants and macros.
*
@ -81,6 +81,9 @@ const F32 OO_LN2 = 1.4426950408889634073599246810019f;
const F32 F_ALMOST_ZERO = 0.0001f;
const F32 F_ALMOST_ONE = 1.0f - F_ALMOST_ZERO;
const F32 GIMBAL_THRESHOLD = 0.000436f; // sets the gimballock threshold 0.025 away from +/-90 degrees
// formula: GIMBAL_THRESHOLD = sin(DEG_TO_RAD * gimbal_threshold_angle);
// BUG: Eliminate in favor of F_APPROXIMATELY_ZERO above?
const F32 FP_MAG_THRESHOLD = 0.0000001f;

447
indra/llmath/llquaternion.cpp
View File

@ -1,4 +1,4 @@
/**
/**
* @file llquaternion.cpp
* @brief LLQuaternion class implementation.
*
@ -58,34 +58,40 @@ LLQuaternion::LLQuaternion(const LLMatrix3 &mat)
LLQuaternion::LLQuaternion(F32 angle, const LLVector4 &vec)
{
LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
v.normalize();
F32 c, s;
c = cosf(angle*0.5f);
s = sinf(angle*0.5f);
mQ[VX] = v.mV[VX] * s;
mQ[VY] = v.mV[VY] * s;
mQ[VZ] = v.mV[VZ] * s;
mQ[VW] = c;
normalize();
F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
if (mag > FP_MAG_THRESHOLD)
{
angle *= 0.5;
F32 c = cosf(angle);
F32 s = sinf(angle) / mag;
mQ[VX] = vec.mV[VX] * s;
mQ[VY] = vec.mV[VY] * s;
mQ[VZ] = vec.mV[VZ] * s;
mQ[VW] = c;
}
else
{
loadIdentity();
}
}
LLQuaternion::LLQuaternion(F32 angle, const LLVector3 &vec)
{
LLVector3 v(vec);
v.normalize();
F32 c, s;
c = cosf(angle*0.5f);
s = sinf(angle*0.5f);
mQ[VX] = v.mV[VX] * s;
mQ[VY] = v.mV[VY] * s;
mQ[VZ] = v.mV[VZ] * s;
mQ[VW] = c;
normalize();
F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
if (mag > FP_MAG_THRESHOLD)
{
angle *= 0.5;
F32 c = cosf(angle);
F32 s = sinf(angle) / mag;
mQ[VX] = vec.mV[VX] * s;
mQ[VY] = vec.mV[VY] * s;
mQ[VZ] = vec.mV[VZ] * s;
mQ[VW] = c;
}
else
{
loadIdentity();
}
}
LLQuaternion::LLQuaternion(const LLVector3 &x_axis,
@ -136,57 +142,61 @@ void LLQuaternion::quantize8(F32 lower, F32 upper)
const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, F32 x, F32 y, F32 z)
{
LLVector3 vec(x, y, z);
vec.normalize();
angle *= 0.5f;
F32 c, s;
c = cosf(angle);
s = sinf(angle);
mQ[VX] = vec.mV[VX]*s;
mQ[VY] = vec.mV[VY]*s;
mQ[VZ] = vec.mV[VZ]*s;
mQ[VW] = c;
normalize();
F32 mag = sqrtf(x * x + y * y + z * z);
if (mag > FP_MAG_THRESHOLD)
{
angle *= 0.5;
F32 c = cosf(angle);
F32 s = sinf(angle) / mag;
mQ[VX] = x * s;
mQ[VY] = y * s;
mQ[VZ] = z * s;
mQ[VW] = c;
}
else
{
loadIdentity();
}
return (*this);
}
const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector3 &vec)
{
LLVector3 v(vec);
v.normalize();
angle *= 0.5f;
F32 c, s;
c = cosf(angle);
s = sinf(angle);
mQ[VX] = v.mV[VX]*s;
mQ[VY] = v.mV[VY]*s;
mQ[VZ] = v.mV[VZ]*s;
mQ[VW] = c;
normalize();
F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
if (mag > FP_MAG_THRESHOLD)
{
angle *= 0.5;
F32 c = cosf(angle);
F32 s = sinf(angle) / mag;
mQ[VX] = vec.mV[VX] * s;
mQ[VY] = vec.mV[VY] * s;
mQ[VZ] = vec.mV[VZ] * s;
mQ[VW] = c;
}
else
{
loadIdentity();
}
return (*this);
}
const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector4 &vec)
{
LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
v.normalize();
F32 c, s;
c = cosf(angle*0.5f);
s = sinf(angle*0.5f);
mQ[VX] = v.mV[VX]*s;
mQ[VY] = v.mV[VY]*s;
mQ[VZ] = v.mV[VZ]*s;
mQ[VW] = c;
normalize();
F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
if (mag > FP_MAG_THRESHOLD)
{
angle *= 0.5;
F32 c = cosf(angle);
F32 s = sinf(angle) / mag;
mQ[VX] = vec.mV[VX] * s;
mQ[VY] = vec.mV[VY] * s;
mQ[VZ] = vec.mV[VZ] * s;
mQ[VW] = c;
}
else
{
loadIdentity();
}
return (*this);
}
@ -219,68 +229,80 @@ const LLQuaternion& LLQuaternion::set(const LLMatrix4 &mat)
// deprecated
const LLQuaternion& LLQuaternion::setQuat(F32 angle, F32 x, F32 y, F32 z)
{
LLVector3 vec(x, y, z);
vec.normalize();
angle *= 0.5f;
F32 c, s;
c = cosf(angle);
s = sinf(angle);
mQ[VX] = vec.mV[VX]*s;
mQ[VY] = vec.mV[VY]*s;
mQ[VZ] = vec.mV[VZ]*s;
mQ[VW] = c;
normalize();
F32 mag = sqrtf(x * x + y * y + z * z);
if (mag > FP_MAG_THRESHOLD)
{
angle *= 0.5;
F32 c = cosf(angle);
F32 s = sinf(angle) / mag;
mQ[VX] = x * s;
mQ[VY] = y * s;
mQ[VZ] = z * s;
mQ[VW] = c;
}
else
{
loadIdentity();
}
return (*this);
}
// deprecated
const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector3 &vec)
{
LLVector3 v(vec);
v.normalize();
angle *= 0.5f;
F32 c, s;
c = cosf(angle);
s = sinf(angle);
mQ[VX] = v.mV[VX]*s;
mQ[VY] = v.mV[VY]*s;
mQ[VZ] = v.mV[VZ]*s;
mQ[VW] = c;
normalize();
F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
if (mag > FP_MAG_THRESHOLD)
{
angle *= 0.5;
F32 c = cosf(angle);
F32 s = sinf(angle) / mag;
mQ[VX] = vec.mV[VX] * s;
mQ[VY] = vec.mV[VY] * s;
mQ[VZ] = vec.mV[VZ] * s;
mQ[VW] = c;
}
else
{
loadIdentity();
}
return (*this);
}
const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector4 &vec)
{
LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
v.normalize();
F32 c, s;
c = cosf(angle*0.5f);
s = sinf(angle*0.5f);
mQ[VX] = v.mV[VX]*s;
mQ[VY] = v.mV[VY]*s;
mQ[VZ] = v.mV[VZ]*s;
mQ[VW] = c;
normalize();
F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
if (mag > FP_MAG_THRESHOLD)
{
angle *= 0.5;
F32 c = cosf(angle);
F32 s = sinf(angle) / mag;
mQ[VX] = vec.mV[VX] * s;
mQ[VY] = vec.mV[VY] * s;
mQ[VZ] = vec.mV[VZ] * s;
mQ[VW] = c;
}
else
{
loadIdentity();
}
return (*this);
}
const LLQuaternion& LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
{
LLMatrix3 rot_mat(roll, pitch, yaw);
rot_mat.orthogonalize();
*this = rot_mat.quaternion();
normalize();
roll *= 0.5f;
pitch *= 0.5f;
yaw *= 0.5f;
F32 sinX = sinf(roll);
F32 cosX = cosf(roll);
F32 sinY = sinf(pitch);
F32 cosY = cosf(pitch);
F32 sinZ = sinf(yaw);
F32 cosZ = cosf(yaw);
mQ[VW] = cosX * cosY * cosZ - sinX * sinY * sinZ;
mQ[VX] = sinX * cosY * cosZ + cosX * sinY * sinZ;
mQ[VY] = cosX * sinY * cosZ - sinX * cosY * sinZ;
mQ[VZ] = cosX * cosY * sinZ + sinX * sinY * cosZ;
return (*this);
}
@ -425,68 +447,44 @@ LLMatrix4 LLQuaternion::getMatrix4(void) const
// calculate the shortest rotation from a to b
void LLQuaternion::shortestArc(const LLVector3 &a, const LLVector3 &b)
{
// Make a local copy of both vectors.
LLVector3 vec_a = a;
LLVector3 vec_b = b;
// Make sure neither vector is zero length. Also normalize
// the vectors while we are at it.
F32 vec_a_mag = vec_a.normalize();
F32 vec_b_mag = vec_b.normalize();
if (vec_a_mag < F_APPROXIMATELY_ZERO ||
vec_b_mag < F_APPROXIMATELY_ZERO)
F32 ab = a * b; // dotproduct
LLVector3 c = a % b; // crossproduct
F32 cc = c * c; // squared length of the crossproduct
if (ab * ab + cc) // test if the arguments have sufficient magnitude
{
// Can't calculate a rotation from this.
// Just return ZERO_ROTATION instead.
loadIdentity();
return;
}
// Create an axis to rotate around, and the cos of the angle to rotate.
LLVector3 axis = vec_a % vec_b;
F32 cos_theta = vec_a * vec_b;
// Check the angle between the vectors to see if they are parallel or anti-parallel.
if (cos_theta > 1.0 - F_APPROXIMATELY_ZERO)
{
// a and b are parallel. No rotation is necessary.
loadIdentity();
}
else if (cos_theta < -1.0 + F_APPROXIMATELY_ZERO)
{
// a and b are anti-parallel.
// Rotate 180 degrees around some orthogonal axis.
// Find the projection of the x-axis onto a, and try
// using the vector between the projection and the x-axis
// as the orthogonal axis.
LLVector3 proj = vec_a.mV[VX] / (vec_a * vec_a) * vec_a;
LLVector3 ortho_axis(1.f, 0.f, 0.f);
ortho_axis -= proj;
// Turn this into an orthonormal axis.
F32 ortho_length = ortho_axis.normalize();
// If the axis' length is 0, then our guess at an orthogonal axis
// was wrong (a is parallel to the x-axis).
if (ortho_length < F_APPROXIMATELY_ZERO)
if (cc > 0.0f) // test if the arguments are (anti)parallel
{
// Use the z-axis instead.
ortho_axis.setVec(0.f, 0.f, 1.f);
F32 s = sqrtf(ab * ab + cc) + ab; // note: don't try to optimize this line
F32 m = 1.0f / sqrtf(cc + s * s); // the inverted magnitude of the quaternion
mQ[VX] = c.mV[VX] * m;
mQ[VY] = c.mV[VY] * m;
mQ[VZ] = c.mV[VZ] * m;
mQ[VW] = s * m;
return;
}
if (ab < 0.0f) // test if the angle is bigger than PI/2 (anti parallel)
{
c = a - b; // the arguments are anti-parallel, we have to choose an axis
F32 m = sqrtf(c.mV[VX] * c.mV[VX] + c.mV[VY] * c.mV[VY]); // the length projected on the XY-plane
if (m > FP_MAG_THRESHOLD)
{
mQ[VX] = -c.mV[VY] / m; // return the quaternion with the axis in the XY-plane
mQ[VY] = c.mV[VX] / m;
mQ[VZ] = 0.0f;
mQ[VW] = 0.0f;
return;
}
else // the vectors are parallel to the Z-axis
{
mQ[VX] = 1.0f; // rotate around the X-axis
mQ[VY] = 0.0f;
mQ[VZ] = 0.0f;
mQ[VW] = 0.0f;
return;
}
}
// Construct a quaternion from this orthonormal axis.
mQ[VX] = ortho_axis.mV[VX];
mQ[VY] = ortho_axis.mV[VY];
mQ[VZ] = ortho_axis.mV[VZ];
mQ[VW] = 0.f;
}
else
{
// a and b are NOT parallel or anti-parallel.
// Return the rotation between these vectors.
F32 theta = (F32)acos(cos_theta);
setAngleAxis(theta, axis);
}
loadIdentity();
}
// constrains rotation to a cone angle specified in radians
@ -838,79 +836,82 @@ LLQuaternion::Order StringToOrder( const char *str )
void LLQuaternion::getAngleAxis(F32* angle, LLVector3 &vec) const
{
F32 cos_a = mQ[VW];
if (cos_a > 1.0f) cos_a = 1.0f;
if (cos_a < -1.0f) cos_a = -1.0f;
F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
if ( fabs( sin_a ) < 0.0005f )
sin_a = 1.0f;
else
sin_a = 1.f/sin_a;
F32 temp_angle = 2.0f * (F32) acos( cos_a );
if (temp_angle > F_PI)
F32 v = sqrtf(mQ[VX] * mQ[VX] + mQ[VY] * mQ[VY] + mQ[VZ] * mQ[VZ]); // length of the vector-component
if (v > FP_MAG_THRESHOLD)
{
// The (angle,axis) pair should never have angles outside [PI, -PI]
// since we want the _shortest_ (angle,axis) solution.
// Since acos is defined for [0, PI], and we multiply by 2.0, we
// can push the angle outside the acceptible range.
// When this happens we set the angle to the other portion of a
// full 2PI rotation, and negate the axis, which reverses the
// direction of the rotation (by the right-hand rule).
*angle = 2.f * F_PI - temp_angle;
vec.mV[VX] = - mQ[VX] * sin_a;
vec.mV[VY] = - mQ[VY] * sin_a;
vec.mV[VZ] = - mQ[VZ] * sin_a;
F32 oomag = 1.0f / v;
F32 w = mQ[VW];
if (mQ[VW] < 0.0f)
{
w = -w; // make VW positive
oomag = -oomag; // invert the axis
}
vec.mV[VX] = mQ[VX] * oomag; // normalize the axis
vec.mV[VY] = mQ[VY] * oomag;
vec.mV[VZ] = mQ[VZ] * oomag;
*angle = 2.0f * atan2f(v, w); // get the angle
}
else
{
*angle = temp_angle;
vec.mV[VX] = mQ[VX] * sin_a;
vec.mV[VY] = mQ[VY] * sin_a;
vec.mV[VZ] = mQ[VZ] * sin_a;
*angle = 0.0f; // no rotation
vec.mV[VX] = 0.0f; // around some dummy axis
vec.mV[VY] = 0.0f;
vec.mV[VZ] = 1.0f;
}
}
// quaternion does not need to be normalized
void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
{
LLMatrix3 rot_mat(*this);
rot_mat.orthogonalize();
rot_mat.getEulerAngles(roll, pitch, yaw);
// // NOTE: LLQuaternion's are actually inverted with respect to
// // the matrices, so this code also assumes inverted quaternions
// // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
// // in reverse order (yaw,pitch,roll).
// F32 x = -mQ[VX], y = -mQ[VY], z = -mQ[VZ], w = mQ[VW];
// F64 m20 = 2.0*(x*z-y*w);
// if (1.0f - fabsf(m20) < F_APPROXIMATELY_ZERO)
// {
// *roll = 0.0f;
// *pitch = (F32)asin(m20);
// *yaw = (F32)atan2(2.0*(x*y-z*w), 1.0 - 2.0*(x*x+z*z));
// }
// else
// {
// *roll = (F32)atan2(-2.0*(y*z+x*w), 1.0-2.0*(x*x+y*y));
// *pitch = (F32)asin(m20);
// *yaw = (F32)atan2(-2.0*(x*y+z*w), 1.0-2.0*(y*y+z*z));
// }
F32 sx = 2 * (mQ[VX] * mQ[VW] - mQ[VY] * mQ[VZ]); // sine of the roll
F32 sy = 2 * (mQ[VY] * mQ[VW] + mQ[VX] * mQ[VZ]); // sine of the pitch
F32 ys = mQ[VW] * mQ[VW] - mQ[VY] * mQ[VY]; // intermediate cosine 1
F32 xz = mQ[VX] * mQ[VX] - mQ[VZ] * mQ[VZ]; // intermediate cosine 2
F32 cx = ys - xz; // cosine of the roll
F32 cy = sqrtf(sx * sx + cx * cx); // cosine of the pitch
if (cy > GIMBAL_THRESHOLD) // no gimbal lock
{
*roll = atan2f(sx, cx);
*pitch = atan2f(sy, cy);
*yaw = atan2f(2 * (mQ[VZ] * mQ[VW] - mQ[VX] * mQ[VY]), ys + xz);
}
else // gimbal lock
{
if (sy > 0)
{
*pitch = F_PI_BY_TWO;
*yaw = 2 * atan2f(mQ[VZ] + mQ[VX], mQ[VW] + mQ[VY]);
}
else
{
*pitch = -F_PI_BY_TWO;
*yaw = 2 * atan2f(mQ[VZ] - mQ[VX], mQ[VW] - mQ[VY]);
}
*roll = 0;
}
}
// Saves space by using the fact that our quaternions are normalized
LLVector3 LLQuaternion::packToVector3() const
{
F32 x = mQ[VX];
F32 y = mQ[VY];
F32 z = mQ[VZ];
F32 w = mQ[VW];
F32 mag = sqrtf(x * x + y * y + z * z + w * w);
if (mag > FP_MAG_THRESHOLD)
{
x /= mag;
y /= mag;
z /= mag; // no need to normalize w, it's not used
}
if( mQ[VW] >= 0 )
{
return LLVector3( mQ[VX], mQ[VY], mQ[VZ] );
return LLVector3( x, y , z );
}
else
{
return LLVector3( -mQ[VX], -mQ[VY], -mQ[VZ] );
return LLVector3( -x, -y, -z );
}
}

50
indra/llmath/llquaternion.h
View File

@ -1,4 +1,4 @@
/**
/**
* @file llquaternion.h
* @brief LLQuaternion class header file.
*
@ -304,43 +304,29 @@ inline const LLQuaternion& LLQuaternion::setQuat(const F32 *q)
return (*this);
}
// There may be a cheaper way that avoids the sqrt.
// Does sin_a = VX*VX + VY*VY + VZ*VZ?
// Copied from Matrix and Quaternion FAQ 1.12
inline void LLQuaternion::getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const
{
F32 cos_a = mQ[VW];
if (cos_a > 1.0f) cos_a = 1.0f;
if (cos_a < -1.0f) cos_a = -1.0f;
F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
if ( fabs( sin_a ) < 0.0005f )
sin_a = 1.0f;
else
sin_a = 1.f/sin_a;
F32 temp_angle = 2.0f * (F32) acos( cos_a );
if (temp_angle > F_PI)
F32 v = sqrtf(mQ[VX] * mQ[VX] + mQ[VY] * mQ[VY] + mQ[VZ] * mQ[VZ]); // length of the vector-component
if (v > FP_MAG_THRESHOLD)
{
// The (angle,axis) pair should never have angles outside [PI, -PI]
// since we want the _shortest_ (angle,axis) solution.
// Since acos is defined for [0, PI], and we multiply by 2.0, we
// can push the angle outside the acceptible range.
// When this happens we set the angle to the other portion of a
// full 2PI rotation, and negate the axis, which reverses the
// direction of the rotation (by the right-hand rule).
*angle = 2.f * F_PI - temp_angle;
*x = - mQ[VX] * sin_a;
*y = - mQ[VY] * sin_a;
*z = - mQ[VZ] * sin_a;
F32 oomag = 1.0f / v;
F32 w = mQ[VW];
if (w < 0.0f)
{
w = -w; // make VW positive
oomag = -oomag; // invert the axis
}
*x = mQ[VX] * oomag; // normalize the axis
*y = mQ[VY] * oomag;
*z = mQ[VZ] * oomag;
*angle = 2.0f * atan2f(v, w); // get the angle
}
else
{
*angle = temp_angle;
*x = mQ[VX] * sin_a;
*y = mQ[VY] * sin_a;
*z = mQ[VZ] * sin_a;
*angle = 0.0f; // no rotation
*x = 0.0f; // around some dummy axis
*y = 0.0f;
*z = 1.0f;
}
}

30
indra/llmath/v3math.h
View File

@ -1,4 +1,4 @@
/**
/**
* @file v3math.h
* @brief LLVector3 class header file.
*
@ -490,9 +490,15 @@ inline F32 dist_vec_squared2D(const LLVector3 &a, const LLVector3 &b)
inline LLVector3 projected_vec(const LLVector3 &a, const LLVector3 &b)
{
LLVector3 project_axis = b;
project_axis.normalize();
return project_axis * (a * project_axis);
F32 bb = b * b;
if (bb > FP_MAG_THRESHOLD * FP_MAG_THRESHOLD)
{
return ((a * b) / bb) * b;
}
else
{
return b.zero;
}
}
inline LLVector3 parallel_component(const LLVector3 &a, const LLVector3 &b)
@ -556,15 +562,13 @@ inline void update_min_max(LLVector3& min, LLVector3& max, const F32* pos)
inline F32 angle_between(const LLVector3& a, const LLVector3& b)
{
LLVector3 an = a;
LLVector3 bn = b;
an.normalize();
bn.normalize();
F32 cosine = an * bn;
F32 angle = (cosine >= 1.0f) ? 0.0f :
(cosine <= -1.0f) ? F_PI :
(F32)acos(cosine);
return angle;
F32 ab = a * b; // dotproduct
if (ab == -0.0f)
{
ab = 0.0f; // get rid of negative zero
}
LLVector3 c = a % b; // crossproduct
return atan2f(sqrtf(c * c), ab); // return the angle
}
inline BOOL are_parallel(const LLVector3 &a, const LLVector3 &b, F32 epsilon)