189 lines
3.5 KiB
C
189 lines
3.5 KiB
C
/*
|
|
===========================================================================
|
|
Copyright (C) 2006 Hermitworks Entertainment Corp
|
|
|
|
This file is part of Space Trader source code.
|
|
|
|
Space Trader source code is free software; you can redistribute it
|
|
and/or modify it under the terms of the GNU General Public License as
|
|
published by the Free Software Foundation; either version 2 of the License,
|
|
or (at your option) any later version.
|
|
|
|
Space Trader source code is distributed in the hope that it will be
|
|
useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with Space Trader source code; if not, write to the Free Software
|
|
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|
===========================================================================
|
|
*/
|
|
|
|
#include "tr_local.h"
|
|
#include "tr_x42-local.h"
|
|
|
|
#ifdef _MSC_VER
|
|
#define DO_INLINE __forceinline
|
|
#else
|
|
#define DO_INLINE ID_INLINE
|
|
#endif
|
|
|
|
NOGLOBALALIAS static DO_INLINE float rsqrt( float x )
|
|
{
|
|
long i;
|
|
float y, r;
|
|
|
|
y = x * 0.5F;
|
|
|
|
i = *(long*)&x;
|
|
i = 0x5F375A86 - (i >> 1);
|
|
r = *(float*)&i;
|
|
|
|
r = r * (1.5F - r * r * y);
|
|
r = r * (1.5F - r * r * y);
|
|
|
|
return r;
|
|
}
|
|
|
|
NOGLOBALALIAS static DO_INLINE float sin_0_HalfPi( float a )
|
|
{
|
|
float s, t;
|
|
|
|
s = a * a;
|
|
|
|
t = -2.39e-08F;
|
|
t *= s;
|
|
|
|
t += 2.7526e-06F;
|
|
t *= s;
|
|
|
|
t += -1.98409e-04F;
|
|
t *= s;
|
|
|
|
t += 8.3333315e-03F;
|
|
t *= s;
|
|
|
|
t += -1.666666664e-01F;
|
|
t *= s;
|
|
|
|
t += 1.0f;
|
|
t *= a;
|
|
|
|
return t;
|
|
}
|
|
|
|
NOGLOBALALIAS static DO_INLINE float atan2_pos( float y, float x )
|
|
{
|
|
float a, d, s, t;
|
|
|
|
if( y > x )
|
|
{
|
|
a = -x / y;
|
|
d = ((float)M_PI / 2.0F);
|
|
}
|
|
else
|
|
{
|
|
a = y / x;
|
|
d = 0.0F;
|
|
}
|
|
|
|
s = a * a;
|
|
|
|
t = 0.0028662257F;
|
|
t *= s;
|
|
|
|
t += -0.0161657367F;
|
|
t *= s;
|
|
|
|
t += 0.0429096138F;
|
|
t *= s;
|
|
|
|
t += -0.0752896400F;
|
|
t *= s;
|
|
|
|
t += 0.1065626393F;
|
|
t *= s;
|
|
|
|
t += -0.1420889944F;
|
|
t *= s;
|
|
|
|
t += 0.1999355085F;
|
|
t *= s;
|
|
|
|
t += -0.3333314528F;
|
|
t *= s;
|
|
|
|
t += 1.0F;
|
|
t *= a;
|
|
|
|
t += d;
|
|
|
|
return t;
|
|
}
|
|
|
|
NOGLOBALALIAS void quat_interp( float * RESTRICT o, const float * RESTRICT a, const float * RESTRICT b, float t )
|
|
{
|
|
float cosTheta;
|
|
float ta, tb;
|
|
bool neg, norm;
|
|
|
|
cosTheta = a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
|
|
|
|
if( cosTheta < 0.0F )
|
|
{
|
|
cosTheta = -cosTheta;
|
|
neg = true;
|
|
}
|
|
else
|
|
neg = false;
|
|
|
|
if( cosTheta >= 1.0F - 1e-5F )
|
|
{
|
|
//quats are very close, just lerp to avoid the division by zero
|
|
ta = 1.0F - t;
|
|
tb = t;
|
|
|
|
norm = false;
|
|
}
|
|
else if( cosTheta >= 0.2F )
|
|
{
|
|
//quats are somewhat close - normalized lerp will do
|
|
ta = 1.0F - t;
|
|
tb = t;
|
|
|
|
norm = true;
|
|
}
|
|
else
|
|
{
|
|
//do full slerp - quickly
|
|
float sinThetaSqr = 1.0F - (cosTheta * cosTheta);
|
|
float sinThetaInv = rsqrt( sinThetaSqr );
|
|
float theta = atan2_pos( sinThetaSqr * sinThetaInv, cosTheta );
|
|
|
|
//we're good to SLERP
|
|
ta = sin_0_HalfPi( (1.0F - t) * theta ) * sinThetaInv;
|
|
tb = sin_0_HalfPi( t * theta ) * sinThetaInv;
|
|
|
|
norm = false;
|
|
}
|
|
|
|
if( neg )
|
|
tb = -tb;
|
|
|
|
o[0] = a[0] * ta + b[0] * tb;
|
|
o[1] = a[1] * ta + b[1] * tb;
|
|
o[2] = a[2] * ta + b[2] * tb;
|
|
o[3] = a[3] * ta + b[3] * tb;
|
|
|
|
if( norm )
|
|
{
|
|
float l = o[0] * o[0] + o[1] * o[1] + o[2] * o[2] + o[3] * o[3];
|
|
l = rsqrt( l );
|
|
|
|
o[0] *= l;
|
|
o[1] *= l;
|
|
o[2] *= l;
|
|
o[3] *= l;
|
|
}
|
|
} |