public VQuaternion(VQuaternion source)
{
X = source.X;
Y = source.Y;
Z = source.Z;
W = source.W;
}
// Originally: void AngleQuaternion( const RadianEuler &angles, Quaternion &outQuat )
// "Purpose: Converts radian-euler axis aligned angles to a quaternion
// Input : *pfAngles - Right-handed Euler angles in radians
// *outQuat - quaternion of form (i,j,k,real)"
public static VQuaternion FromRadianEulerAngles(Vector3 rangles) // In RADIANS
{
// NOTE: The ordering here is *different* from the AngleQuaternion below
// because p, y, r are not in the same locations in QAngle + RadianEuler. Yay!
float sy = (float)Math.Sin(rangles.Z * 0.5f);
float cy = (float)Math.Cos(rangles.Z * 0.5f);
float sp = (float)Math.Sin(rangles.Y * 0.5f);
float cp = (float)Math.Cos(rangles.Y * 0.5f);
float sr = (float)Math.Sin(rangles.X * 0.5f);
float cr = (float)Math.Cos(rangles.X * 0.5f);
VQuaternion outQuat = VQuaternion.Zero;
// NJS: for some reason VC6 wasn't recognizing the common subexpressions:
float srXcp = sr * cp, crXsp = cr * sp;
outQuat.X = srXcp * cy - crXsp * sy; // X
outQuat.Y = crXsp * cy + srXcp * sy; // Y
float crXcp = cr * cp, srXsp = sr * sp;
outQuat.Z = crXcp * sy - srXsp * cy; // Z
outQuat.W = crXcp * cy + srXsp * sy; // W (real component)
return(outQuat);
}
// Originally: void QuaternionSM(float s, const Quaternion &p, const Quaternion &q, Quaternion &qt )
// "Purpose: qt = ( s * p ) * q"
public VQuaternion ScaledMultipliedWith(float s, VQuaternion q)
{
VQuaternion p1 = this.ScaledBy(s);
VQuaternion q1 = p1.MultipliedWith(q);
VQuaternion qt = q1.Normalized();
return(qt);
}
public override bool Equals(object b)
{
if (!(b is VQuaternion))
{
return(false);
}
VQuaternion bq = (VQuaternion)b;
return(X == bq.X && Y == bq.Y && Z == bq.Z && W == bq.W);
}
// Originally: void QuaternionMult( const Quaternion &p, const Quaternion &q, Quaternion &qt )
// "qt = p * q"
public VQuaternion MultipliedWith(VQuaternion q)
{
// decide if one of the quaternions is backwards
VQuaternion q2 = Align(this, q);
VQuaternion qt = VQuaternion.Zero;
qt.X = X * q2.W + Y * q2.Z - Z * q2.Y + W * q2.X;
qt.Y = -X * q2.Z + Y * q2.W + Z * q2.X + W * q2.Y;
qt.Z = X * q2.Y - Y * q2.X + Z * q2.W + W * q2.Z;
qt.W = -X * q2.X - Y * q2.Y - Z * q2.Z + W * q2.W;
return(qt);
}
// Originally: float QuaternionNormalize(Quaternion &q )
// "Make sure the quaternion is of unit length"
public VQuaternion Normalized()
{
float radius, iradius;
radius = (X * X) + (Y * Y) + (Z * Z) + (W * W);
VQuaternion q2 = new VQuaternion(this);
if (radius != 0) // > FLT_EPSILON && ((radius < 1.0f - 4*FLT_EPSILON) || (radius > 1.0f + 4*FLT_EPSILON))
{
radius = (float)Math.Sqrt(radius);
iradius = 1.0f / radius;
q2.W *= iradius;
q2.Z *= iradius;
q2.Y *= iradius;
q2.X *= iradius;
}
return(q2);
}
// Originally: void QuaternionScale( const Quaternion &p, float t, Quaternion &q )
public VQuaternion ScaledBy(float t)
{
float r;
// FIXME: nick, this isn't overly sensitive to accuracy, and it may be faster to
// use the cos part (w) of the quaternion (sin(omega)*N,cos(omega)) to figure the new scale.
// I guess in the 3 years since the 2004 SDK, Nick still wouldn't let anyone replace his code
float sinom = (float)Math.Sqrt((X * X) + (Y * Y) + (Z * Z));
sinom = (float)Math.Min(sinom, 1f);
float sinsom = (float)Math.Sin(Math.Asin(sinom) * t);
t = sinsom / (sinom + float.Epsilon);
VQuaternion q = new VQuaternion(X * t, Y * t, Z * t, W);
// rescale rotation
r = 1.0f - sinsom * sinsom;
// Assert( r >= 0 );
if (r < 0.0f)
{
r = 0.0f;
}
r = (float)Math.Sqrt(r);
// keep sign of rotation
if (W < 0)
{
q.W = -r;
}
else
{
q.W = r;
}
return(q);
}
// Originally: void QuaternionAlign( const Quaternion &p, const Quaternion &q, Quaternion &qt )
// "make sure quaternions are within 180 degrees of one another, if not, reverse q"
public static VQuaternion Align(VQuaternion p, VQuaternion q)
{
VQuaternion qt = new VQuaternion(q);
// "decide if one of the quaternions is backwards"
float a = (p.X - q.X) * (p.X - q.X)
+ (p.Y - q.Y) * (p.Y - q.Y)
+ (p.Z - q.Z) * (p.Z - q.Z)
+ (p.W - q.W) * (p.W - q.W);
float b = (p.X + q.X) * (p.X + q.X)
+ (p.Y + q.Y) * (p.Y + q.Y)
+ (p.Z + q.Z) * (p.Z + q.Z)
+ (p.W + q.W) * (p.W + q.W);
if (a > b)
{
qt.X = -q.X;
qt.Y = -q.Y;
qt.Z = -q.Z;
qt.W = -q.W;
}
return(qt);
}
// Originally: void QuaternionSMAngles(float s, Quaternion const &p, Quaternion const &q, RadianEuler &angles )
// "overlay
// studiomdl : delta = (-1 * base_anim ) * new_anim
// engine : result = base_anim * (w * delta)"
public static Vector3 SMAngles(float s, VQuaternion p, VQuaternion q)
{
VQuaternion qt = p.ScaledMultipliedWith(s, q);
return(qt.ToRadianAngles());
}
