Code:
/ Net / Net / 3.5.50727.3053 / DEVDIV / depot / DevDiv / releases / Orcas / SP / wpf / src / Core / CSharp / System / Windows / Media3D / Quaternion.cs / 1 / Quaternion.cs
//----------------------------------------------------------------------------
//
//
// Copyright (C) Microsoft Corporation. All rights reserved.
//
//
//
// Description: 3D quaternion implementation.
//
// See spec at http://avalon/medialayer/Specifications/Avalon3D%20API%20Spec.mht
//
// History:
// 06/02/2003 : t-gregr - Created
//
// NOTE: The field _isNotDistinguishedIdentity is a work-around to the
// problem that we can't define a default constructor that sets
// _w to 1. So the default constructor sets all fields to 0 or
// false, and we interpret _isNotDistinguishedIdentity as follows
//
// If false, the quaternion is the identity 0,0,0,1 even though
// the member fields are 0,0,0,0.
//
// If true, the quaternion has the value given by its member fields.
//
// Don't mess it up!
//---------------------------------------------------------------------------
using MS.Internal;
using System;
using System.Collections;
using System.ComponentModel;
using System.Diagnostics;
using System.Globalization;
using System.Reflection;
using System.Runtime.InteropServices;
using System.ComponentModel.Design.Serialization;
using System.Windows.Markup;
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Media.Composition;
// These types are aliased to match the unamanaged names used in interop
using BOOL = System.Boolean;
using WORD = System.UInt16;
using Float = System.Single;
using SR=MS.Internal.PresentationCore.SR;
using SRID=MS.Internal.PresentationCore.SRID;
namespace System.Windows.Media.Media3D
{
///
/// Quaternions.
/// Quaternions are distinctly 3D entities that represent rotation in three dimensions.
/// Their power comes in being able to interpolate (and thus animate) between
/// quaternions to achieve a smooth, reliable interpolation.
/// The default quaternion is the identity.
///
public partial struct Quaternion : IFormattable
{
//-----------------------------------------------------
//
// Constructors
//
//-----------------------------------------------------
#region Constructors
///
/// Constructor that sets quaternion's initial values.
///
/// Value of the X coordinate of the new quaternion.
/// Value of the Y coordinate of the new quaternion.
/// Value of the Z coordinate of the new quaternion.
/// Value of the W coordinate of the new quaternion.
public Quaternion(double x, double y, double z, double w)
{
_x = x;
_y = y;
_z = z;
_w = w;
_isNotDistinguishedIdentity = true;
}
///
/// Constructs a quaternion via specified axis of rotation and an angle.
/// Throws an InvalidOperationException if given (0,0,0) as axis vector.
///
/// Vector representing axis of rotation.
/// Angle to turn around the given axis (in degrees).
public Quaternion(Vector3D axisOfRotation, double angleInDegrees)
{
angleInDegrees %= 360.0; // Doing the modulo before converting to radians reduces total error
double angleInRadians = angleInDegrees * (Math.PI / 180.0);
double length = axisOfRotation.Length;
if (length == 0)
throw new System.InvalidOperationException(SR.Get(SRID.Quaternion_ZeroAxisSpecified));
Vector3D v = (axisOfRotation / length) * Math.Sin(0.5 * angleInRadians);
_x = v.X;
_y = v.Y;
_z = v.Z;
_w = Math.Cos(0.5 * angleInRadians);
_isNotDistinguishedIdentity = true;
}
#endregion Constructors
//------------------------------------------------------
//
// Public Methods
//
//-----------------------------------------------------
#region Public Methods
///
/// Identity quaternion
///
public static Quaternion Identity
{
get
{
return s_identity;
}
}
///
/// Retrieves quaternion's axis.
///
public Vector3D Axis
{
// q = M [cos(Q/2), sin(Q /2)v]
// axis = sin(Q/2)v
// angle = cos(Q/2)
// M is magnitude
get
{
// Handle identity (where axis is indeterminate) by
// returning arbitrary axis.
if (IsDistinguishedIdentity || (_x == 0 && _y == 0 && _z == 0))
{
return new Vector3D(0,1,0);
}
else
{
Vector3D v = new Vector3D(_x, _y, _z);
v.Normalize();
return v;
}
}
}
///
/// Retrieves quaternion's angle.
///
public double Angle
{
get
{
if (IsDistinguishedIdentity)
{
return 0;
}
// Magnitude of quaternion times sine and cosine
double msin = Math.Sqrt(_x*_x + _y*_y + _z*_z);
double mcos = _w;
if (!(msin <= Double.MaxValue))
{
// Overflowed probably in squaring, so let's scale
// the values. We don't need to include _w in the
// scale factor because we're not going to square
// it.
double maxcoeff = Math.Max(Math.Abs(_x),Math.Max(Math.Abs(_y),Math.Abs(_z)));
double x = _x/maxcoeff;
double y = _y/maxcoeff;
double z = _z/maxcoeff;
msin = Math.Sqrt(x*x + y*y + z*z);
// Scale mcos too.
mcos = _w/maxcoeff;
}
// Atan2 is better than acos. (More precise and more efficient.)
return Math.Atan2(msin,mcos) * (360.0 / Math.PI);
}
}
///
/// Returns whether the quaternion is normalized (i.e. has a magnitude of 1).
///
public bool IsNormalized
{
get
{
if (IsDistinguishedIdentity)
{
return true;
}
double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
return DoubleUtil.IsOne(norm2);
}
}
///
/// Tests whether or not a given quaternion is an identity quaternion.
///
public bool IsIdentity
{
get
{
return IsDistinguishedIdentity || (_x == 0 && _y == 0 && _z == 0 && _w == 1);
}
}
///
/// Relaces quaternion with its conjugate
///
public void Conjugate()
{
if (IsDistinguishedIdentity)
{
return;
}
// Conjugate([x,y,z,w]) = [-x,-y,-z,w]
_x = -_x;
_y = -_y;
_z = -_z;
}
///
/// Replaces quaternion with its inverse
///
public void Invert()
{
if (IsDistinguishedIdentity)
{
return;
}
// Inverse = Conjugate / Norm Squared
Conjugate();
double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
_x /= norm2;
_y /= norm2;
_z /= norm2;
_w /= norm2;
}
///
/// Normalizes this quaternion.
///
public void Normalize()
{
if (IsDistinguishedIdentity)
{
return;
}
double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
if (norm2 > Double.MaxValue)
{
// Handle overflow in computation of norm2
double rmax = 1.0/Max(Math.Abs(_x),
Math.Abs(_y),
Math.Abs(_z),
Math.Abs(_w));
_x *= rmax;
_y *= rmax;
_z *= rmax;
_w *= rmax;
norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
}
double normInverse = 1.0 / Math.Sqrt(norm2);
_x *= normInverse;
_y *= normInverse;
_z *= normInverse;
_w *= normInverse;
}
///
/// Quaternion addition.
///
/// First quaternion being added.
/// Second quaternion being added.
/// Result of addition.
public static Quaternion operator +(Quaternion left, Quaternion right)
{
if (right.IsDistinguishedIdentity)
{
if (left.IsDistinguishedIdentity)
{
return new Quaternion(0,0,0,2);
}
else
{
// We know left is not distinguished identity here.
left._w += 1;
return left;
}
}
else if (left.IsDistinguishedIdentity)
{
// We know right is not distinguished identity here.
right._w += 1;
return right;
}
else
{
return new Quaternion(left._x + right._x,
left._y + right._y,
left._z + right._z,
left._w + right._w);
}
}
///
/// Quaternion addition.
///
/// First quaternion being added.
/// Second quaternion being added.
/// Result of addition.
public static Quaternion Add(Quaternion left, Quaternion right)
{
return (left + right);
}
///
/// Quaternion subtraction.
///
/// Quaternion to subtract from.
/// Quaternion to subtract from the first quaternion.
/// Result of subtraction.
public static Quaternion operator -(Quaternion left, Quaternion right)
{
if (right.IsDistinguishedIdentity)
{
if (left.IsDistinguishedIdentity)
{
return new Quaternion(0,0,0,0);
}
else
{
// We know left is not distinguished identity here.
left._w -= 1;
return left;
}
}
else if (left.IsDistinguishedIdentity)
{
// We know right is not distinguished identity here.
return new Quaternion(-right._x, -right._y, -right._z, 1 - right._w);
}
else
{
return new Quaternion(left._x - right._x,
left._y - right._y,
left._z - right._z,
left._w - right._w);
}
}
///
/// Quaternion subtraction.
///
/// Quaternion to subtract from.
/// Quaternion to subtract from the first quaternion.
/// Result of subtraction.
public static Quaternion Subtract(Quaternion left, Quaternion right)
{
return (left - right);
}
///
/// Quaternion multiplication.
///
/// First quaternion.
/// Second quaternion.
/// Result of multiplication.
public static Quaternion operator *(Quaternion left, Quaternion right)
{
if (left.IsDistinguishedIdentity)
{
return right;
}
if (right.IsDistinguishedIdentity)
{
return left;
}
double x = left._w * right._x + left._x * right._w + left._y * right._z - left._z * right._y;
double y = left._w * right._y + left._y * right._w + left._z * right._x - left._x * right._z;
double z = left._w * right._z + left._z * right._w + left._x * right._y - left._y * right._x;
double w = left._w * right._w - left._x * right._x - left._y * right._y - left._z * right._z;
Quaternion result = new Quaternion(x,y,z,w);
return result;
}
///
/// Quaternion multiplication.
///
/// First quaternion.
/// Second quaternion.
/// Result of multiplication.
public static Quaternion Multiply(Quaternion left, Quaternion right)
{
return left * right;
}
///
/// Scale this quaternion by a scalar.
///
/// Value to scale by.
private void Scale( double scale )
{
if (IsDistinguishedIdentity)
{
_w = scale;
IsDistinguishedIdentity = false;
return;
}
_x *= scale;
_y *= scale;
_z *= scale;
_w *= scale;
}
///
/// Return length of quaternion.
///
private double Length()
{
if (IsDistinguishedIdentity)
{
return 1;
}
double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
if (!(norm2 <= Double.MaxValue))
{
// Do this the slow way to avoid squaring large
// numbers since the length of many quaternions is
// representable even if the squared length isn't. Of
// course some lengths aren't representable because
// the length can be up to twice as big as the largest
// coefficient.
double max = Math.Max(Math.Max(Math.Abs(_x),Math.Abs(_y)),
Math.Max(Math.Abs(_z),Math.Abs(_w)));
double x = _x/max;
double y = _y/max;
double z = _z/max;
double w = _w/max;
double smallLength = Math.Sqrt(x*x+y*y+z*z+w*w);
// Return length of this smaller vector times the scale we applied originally.
return smallLength * max;
}
return Math.Sqrt(norm2);
}
///
/// Smoothly interpolate between the two given quaternions using Spherical
/// Linear Interpolation (SLERP).
///
/// First quaternion for interpolation.
/// Second quaternion for interpolation.
/// Interpolation coefficient.
/// SLERP-interpolated quaternion between the two given quaternions.
public static Quaternion Slerp(Quaternion from, Quaternion to, double t)
{
return Slerp(from, to, t, /* useShortestPath = */ true);
}
///
/// Smoothly interpolate between the two given quaternions using Spherical
/// Linear Interpolation (SLERP).
///
/// First quaternion for interpolation.
/// Second quaternion for interpolation.
/// Interpolation coefficient.
/// If true, Slerp will automatically flip the sign of
/// the destination Quaternion to ensure the shortest path is taken.
/// SLERP-interpolated quaternion between the two given quaternions.
public static Quaternion Slerp(Quaternion from, Quaternion to, double t, bool useShortestPath)
{
if (from.IsDistinguishedIdentity)
{
from._w = 1;
}
if (to.IsDistinguishedIdentity)
{
to._w = 1;
}
double cosOmega;
double scaleFrom, scaleTo;
// Normalize inputs and stash their lengths
double lengthFrom = from.Length();
double lengthTo = to.Length();
from.Scale(1/lengthFrom);
to.Scale(1/lengthTo);
// Calculate cos of omega.
cosOmega = from._x*to._x + from._y*to._y + from._z*to._z + from._w*to._w;
if (useShortestPath)
{
// If we are taking the shortest path we flip the signs to ensure that
// cosOmega will be positive.
if (cosOmega < 0.0)
{
cosOmega = -cosOmega;
to._x = -to._x;
to._y = -to._y;
to._z = -to._z;
to._w = -to._w;
}
}
else
{
// If we are not taking the UseShortestPath we clamp cosOmega to
// -1 to stay in the domain of Math.Acos below.
if (cosOmega < -1.0)
{
cosOmega = -1.0;
}
}
// Clamp cosOmega to [-1,1] to stay in the domain of Math.Acos below.
// The logic above has either flipped the sign of cosOmega to ensure it
// is positive or clamped to -1 aready. We only need to worry about the
// upper limit here.
if (cosOmega > 1.0)
{
cosOmega = 1.0;
}
Debug.Assert(!(cosOmega < -1.0) && !(cosOmega > 1.0),
"cosOmega should be clamped to [-1,1]");
// The mainline algorithm doesn't work for extreme
// cosine values. For large cosine we have a better
// fallback hence the asymmetric limits.
const double maxCosine = 1.0 - 1e-6;
const double minCosine = 1e-10 - 1.0;
// Calculate scaling coefficients.
if (cosOmega > maxCosine)
{
// Quaternions are too close - use linear interpolation.
scaleFrom = 1.0 - t;
scaleTo = t;
}
else if (cosOmega < minCosine)
{
// Quaternions are nearly opposite, so we will pretend to
// is exactly -from.
// First assign arbitrary perpendicular to "to".
to = new Quaternion(-from.Y, from.X, -from.W, from.Z);
double theta = t * Math.PI;
scaleFrom = Math.Cos(theta);
scaleTo = Math.Sin(theta);
}
else
{
// Standard case - use SLERP interpolation.
double omega = Math.Acos(cosOmega);
double sinOmega = Math.Sqrt(1.0 - cosOmega*cosOmega);
scaleFrom = Math.Sin((1.0 - t) * omega) / sinOmega;
scaleTo = Math.Sin(t * omega) / sinOmega;
}
// We want the magnitude of the output quaternion to be
// multiplicatively interpolated between the input
// magnitudes, i.e. lengthOut = lengthFrom * (lengthTo/lengthFrom)^t
// = lengthFrom ^ (1-t) * lengthTo ^ t
double lengthOut = lengthFrom * Math.Pow(lengthTo/lengthFrom, t);
scaleFrom *= lengthOut;
scaleTo *= lengthOut;
return new Quaternion(scaleFrom*from._x + scaleTo*to._x,
scaleFrom*from._y + scaleTo*to._y,
scaleFrom*from._z + scaleTo*to._z,
scaleFrom*from._w + scaleTo*to._w);
}
#endregion Public Methods
#region Private Methods
static private double Max(double a, double b, double c, double d)
{
if (b > a)
a = b;
if (c > a)
a = c;
if (d > a)
a = d;
return a;
}
#endregion Private Methods
//------------------------------------------------------
//
// Public Properties
//
//------------------------------------------------------
#region Public Properties
///
/// X - Default value is 0.
///
public double X
{
get
{
return _x;
}
set
{
if (IsDistinguishedIdentity)
{
this = s_identity;
IsDistinguishedIdentity = false;
}
_x = value;
}
}
///
/// Y - Default value is 0.
///
public double Y
{
get
{
return _y;
}
set
{
if (IsDistinguishedIdentity)
{
this = s_identity;
IsDistinguishedIdentity = false;
}
_y = value;
}
}
///
/// Z - Default value is 0.
///
public double Z
{
get
{
return _z;
}
set
{
if (IsDistinguishedIdentity)
{
this = s_identity;
IsDistinguishedIdentity = false;
}
_z = value;
}
}
///
/// W - Default value is 1.
///
public double W
{
get
{
if (IsDistinguishedIdentity)
{
return 1.0;
}
else
{
return _w;
}
}
set
{
if (IsDistinguishedIdentity)
{
this = s_identity;
IsDistinguishedIdentity = false;
}
_w = value;
}
}
#endregion Public Properties
//-----------------------------------------------------
//
// Internal Fields
//
//------------------------------------------------------
#region Internal Fields
internal double _x;
internal double _y;
internal double _z;
internal double _w;
#endregion Internal Fields
#region Private Fields and Properties
// If this bool is false then we are a default quaternion with
// all doubles equal to zero, but should be treated as
// identity.
private bool _isNotDistinguishedIdentity;
private bool IsDistinguishedIdentity
{
get
{
return !_isNotDistinguishedIdentity;
}
set
{
_isNotDistinguishedIdentity = !value;
}
}
private static int GetIdentityHashCode()
{
// This code is called only once.
double zero = 0;
double one = 1;
// return zero.GetHashCode() ^ zero.GetHashCode() ^ zero.GetHashCode() ^ one.GetHashCode();
// But this expression can be simplified because the first two hash codes cancel.
return zero.GetHashCode() ^ one.GetHashCode();
}
private static Quaternion GetIdentity()
{
// This code is called only once.
Quaternion q = new Quaternion(0,0,0,1);
q.IsDistinguishedIdentity = true;
return q;
}
// Hash code for identity.
private static int c_identityHashCode = GetIdentityHashCode();
// Default identity
private static Quaternion s_identity = GetIdentity();
#endregion Private Fields and Properties
}
}
// File provided for Reference Use Only by Microsoft Corporation (c) 2007.
// Copyright (c) Microsoft Corporation. All rights reserved.
//----------------------------------------------------------------------------
//
//
// Copyright (C) Microsoft Corporation. All rights reserved.
//
//
//
// Description: 3D quaternion implementation.
//
// See spec at http://avalon/medialayer/Specifications/Avalon3D%20API%20Spec.mht
//
// History:
// 06/02/2003 : t-gregr - Created
//
// NOTE: The field _isNotDistinguishedIdentity is a work-around to the
// problem that we can't define a default constructor that sets
// _w to 1. So the default constructor sets all fields to 0 or
// false, and we interpret _isNotDistinguishedIdentity as follows
//
// If false, the quaternion is the identity 0,0,0,1 even though
// the member fields are 0,0,0,0.
//
// If true, the quaternion has the value given by its member fields.
//
// Don't mess it up!
//---------------------------------------------------------------------------
using MS.Internal;
using System;
using System.Collections;
using System.ComponentModel;
using System.Diagnostics;
using System.Globalization;
using System.Reflection;
using System.Runtime.InteropServices;
using System.ComponentModel.Design.Serialization;
using System.Windows.Markup;
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Media.Composition;
// These types are aliased to match the unamanaged names used in interop
using BOOL = System.Boolean;
using WORD = System.UInt16;
using Float = System.Single;
using SR=MS.Internal.PresentationCore.SR;
using SRID=MS.Internal.PresentationCore.SRID;
namespace System.Windows.Media.Media3D
{
///
/// Quaternions.
/// Quaternions are distinctly 3D entities that represent rotation in three dimensions.
/// Their power comes in being able to interpolate (and thus animate) between
/// quaternions to achieve a smooth, reliable interpolation.
/// The default quaternion is the identity.
///
public partial struct Quaternion : IFormattable
{
//-----------------------------------------------------
//
// Constructors
//
//-----------------------------------------------------
#region Constructors
///
/// Constructor that sets quaternion's initial values.
///
/// Value of the X coordinate of the new quaternion.
/// Value of the Y coordinate of the new quaternion.
/// Value of the Z coordinate of the new quaternion.
/// Value of the W coordinate of the new quaternion.
public Quaternion(double x, double y, double z, double w)
{
_x = x;
_y = y;
_z = z;
_w = w;
_isNotDistinguishedIdentity = true;
}
///
/// Constructs a quaternion via specified axis of rotation and an angle.
/// Throws an InvalidOperationException if given (0,0,0) as axis vector.
///
/// Vector representing axis of rotation.
/// Angle to turn around the given axis (in degrees).
public Quaternion(Vector3D axisOfRotation, double angleInDegrees)
{
angleInDegrees %= 360.0; // Doing the modulo before converting to radians reduces total error
double angleInRadians = angleInDegrees * (Math.PI / 180.0);
double length = axisOfRotation.Length;
if (length == 0)
throw new System.InvalidOperationException(SR.Get(SRID.Quaternion_ZeroAxisSpecified));
Vector3D v = (axisOfRotation / length) * Math.Sin(0.5 * angleInRadians);
_x = v.X;
_y = v.Y;
_z = v.Z;
_w = Math.Cos(0.5 * angleInRadians);
_isNotDistinguishedIdentity = true;
}
#endregion Constructors
//------------------------------------------------------
//
// Public Methods
//
//-----------------------------------------------------
#region Public Methods
///
/// Identity quaternion
///
public static Quaternion Identity
{
get
{
return s_identity;
}
}
///
/// Retrieves quaternion's axis.
///
public Vector3D Axis
{
// q = M [cos(Q/2), sin(Q /2)v]
// axis = sin(Q/2)v
// angle = cos(Q/2)
// M is magnitude
get
{
// Handle identity (where axis is indeterminate) by
// returning arbitrary axis.
if (IsDistinguishedIdentity || (_x == 0 && _y == 0 && _z == 0))
{
return new Vector3D(0,1,0);
}
else
{
Vector3D v = new Vector3D(_x, _y, _z);
v.Normalize();
return v;
}
}
}
///
/// Retrieves quaternion's angle.
///
public double Angle
{
get
{
if (IsDistinguishedIdentity)
{
return 0;
}
// Magnitude of quaternion times sine and cosine
double msin = Math.Sqrt(_x*_x + _y*_y + _z*_z);
double mcos = _w;
if (!(msin <= Double.MaxValue))
{
// Overflowed probably in squaring, so let's scale
// the values. We don't need to include _w in the
// scale factor because we're not going to square
// it.
double maxcoeff = Math.Max(Math.Abs(_x),Math.Max(Math.Abs(_y),Math.Abs(_z)));
double x = _x/maxcoeff;
double y = _y/maxcoeff;
double z = _z/maxcoeff;
msin = Math.Sqrt(x*x + y*y + z*z);
// Scale mcos too.
mcos = _w/maxcoeff;
}
// Atan2 is better than acos. (More precise and more efficient.)
return Math.Atan2(msin,mcos) * (360.0 / Math.PI);
}
}
///
/// Returns whether the quaternion is normalized (i.e. has a magnitude of 1).
///
public bool IsNormalized
{
get
{
if (IsDistinguishedIdentity)
{
return true;
}
double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
return DoubleUtil.IsOne(norm2);
}
}
///
/// Tests whether or not a given quaternion is an identity quaternion.
///
public bool IsIdentity
{
get
{
return IsDistinguishedIdentity || (_x == 0 && _y == 0 && _z == 0 && _w == 1);
}
}
///
/// Relaces quaternion with its conjugate
///
public void Conjugate()
{
if (IsDistinguishedIdentity)
{
return;
}
// Conjugate([x,y,z,w]) = [-x,-y,-z,w]
_x = -_x;
_y = -_y;
_z = -_z;
}
///
/// Replaces quaternion with its inverse
///
public void Invert()
{
if (IsDistinguishedIdentity)
{
return;
}
// Inverse = Conjugate / Norm Squared
Conjugate();
double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
_x /= norm2;
_y /= norm2;
_z /= norm2;
_w /= norm2;
}
///
/// Normalizes this quaternion.
///
public void Normalize()
{
if (IsDistinguishedIdentity)
{
return;
}
double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
if (norm2 > Double.MaxValue)
{
// Handle overflow in computation of norm2
double rmax = 1.0/Max(Math.Abs(_x),
Math.Abs(_y),
Math.Abs(_z),
Math.Abs(_w));
_x *= rmax;
_y *= rmax;
_z *= rmax;
_w *= rmax;
norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
}
double normInverse = 1.0 / Math.Sqrt(norm2);
_x *= normInverse;
_y *= normInverse;
_z *= normInverse;
_w *= normInverse;
}
///
/// Quaternion addition.
///
/// First quaternion being added.
/// Second quaternion being added.
/// Result of addition.
public static Quaternion operator +(Quaternion left, Quaternion right)
{
if (right.IsDistinguishedIdentity)
{
if (left.IsDistinguishedIdentity)
{
return new Quaternion(0,0,0,2);
}
else
{
// We know left is not distinguished identity here.
left._w += 1;
return left;
}
}
else if (left.IsDistinguishedIdentity)
{
// We know right is not distinguished identity here.
right._w += 1;
return right;
}
else
{
return new Quaternion(left._x + right._x,
left._y + right._y,
left._z + right._z,
left._w + right._w);
}
}
///
/// Quaternion addition.
///
/// First quaternion being added.
/// Second quaternion being added.
/// Result of addition.
public static Quaternion Add(Quaternion left, Quaternion right)
{
return (left + right);
}
///
/// Quaternion subtraction.
///
/// Quaternion to subtract from.
/// Quaternion to subtract from the first quaternion.
/// Result of subtraction.
public static Quaternion operator -(Quaternion left, Quaternion right)
{
if (right.IsDistinguishedIdentity)
{
if (left.IsDistinguishedIdentity)
{
return new Quaternion(0,0,0,0);
}
else
{
// We know left is not distinguished identity here.
left._w -= 1;
return left;
}
}
else if (left.IsDistinguishedIdentity)
{
// We know right is not distinguished identity here.
return new Quaternion(-right._x, -right._y, -right._z, 1 - right._w);
}
else
{
return new Quaternion(left._x - right._x,
left._y - right._y,
left._z - right._z,
left._w - right._w);
}
}
///
/// Quaternion subtraction.
///
/// Quaternion to subtract from.
/// Quaternion to subtract from the first quaternion.
/// Result of subtraction.
public static Quaternion Subtract(Quaternion left, Quaternion right)
{
return (left - right);
}
///
/// Quaternion multiplication.
///
/// First quaternion.
/// Second quaternion.
/// Result of multiplication.
public static Quaternion operator *(Quaternion left, Quaternion right)
{
if (left.IsDistinguishedIdentity)
{
return right;
}
if (right.IsDistinguishedIdentity)
{
return left;
}
double x = left._w * right._x + left._x * right._w + left._y * right._z - left._z * right._y;
double y = left._w * right._y + left._y * right._w + left._z * right._x - left._x * right._z;
double z = left._w * right._z + left._z * right._w + left._x * right._y - left._y * right._x;
double w = left._w * right._w - left._x * right._x - left._y * right._y - left._z * right._z;
Quaternion result = new Quaternion(x,y,z,w);
return result;
}
///
/// Quaternion multiplication.
///
/// First quaternion.
/// Second quaternion.
/// Result of multiplication.
public static Quaternion Multiply(Quaternion left, Quaternion right)
{
return left * right;
}
///
/// Scale this quaternion by a scalar.
///
/// Value to scale by.
private void Scale( double scale )
{
if (IsDistinguishedIdentity)
{
_w = scale;
IsDistinguishedIdentity = false;
return;
}
_x *= scale;
_y *= scale;
_z *= scale;
_w *= scale;
}
///
/// Return length of quaternion.
///
private double Length()
{
if (IsDistinguishedIdentity)
{
return 1;
}
double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
if (!(norm2 <= Double.MaxValue))
{
// Do this the slow way to avoid squaring large
// numbers since the length of many quaternions is
// representable even if the squared length isn't. Of
// course some lengths aren't representable because
// the length can be up to twice as big as the largest
// coefficient.
double max = Math.Max(Math.Max(Math.Abs(_x),Math.Abs(_y)),
Math.Max(Math.Abs(_z),Math.Abs(_w)));
double x = _x/max;
double y = _y/max;
double z = _z/max;
double w = _w/max;
double smallLength = Math.Sqrt(x*x+y*y+z*z+w*w);
// Return length of this smaller vector times the scale we applied originally.
return smallLength * max;
}
return Math.Sqrt(norm2);
}
///
/// Smoothly interpolate between the two given quaternions using Spherical
/// Linear Interpolation (SLERP).
///
/// First quaternion for interpolation.
/// Second quaternion for interpolation.
/// Interpolation coefficient.
/// SLERP-interpolated quaternion between the two given quaternions.
public static Quaternion Slerp(Quaternion from, Quaternion to, double t)
{
return Slerp(from, to, t, /* useShortestPath = */ true);
}
///
/// Smoothly interpolate between the two given quaternions using Spherical
/// Linear Interpolation (SLERP).
///
/// First quaternion for interpolation.
/// Second quaternion for interpolation.
/// Interpolation coefficient.
/// If true, Slerp will automatically flip the sign of
/// the destination Quaternion to ensure the shortest path is taken.
/// SLERP-interpolated quaternion between the two given quaternions.
public static Quaternion Slerp(Quaternion from, Quaternion to, double t, bool useShortestPath)
{
if (from.IsDistinguishedIdentity)
{
from._w = 1;
}
if (to.IsDistinguishedIdentity)
{
to._w = 1;
}
double cosOmega;
double scaleFrom, scaleTo;
// Normalize inputs and stash their lengths
double lengthFrom = from.Length();
double lengthTo = to.Length();
from.Scale(1/lengthFrom);
to.Scale(1/lengthTo);
// Calculate cos of omega.
cosOmega = from._x*to._x + from._y*to._y + from._z*to._z + from._w*to._w;
if (useShortestPath)
{
// If we are taking the shortest path we flip the signs to ensure that
// cosOmega will be positive.
if (cosOmega < 0.0)
{
cosOmega = -cosOmega;
to._x = -to._x;
to._y = -to._y;
to._z = -to._z;
to._w = -to._w;
}
}
else
{
// If we are not taking the UseShortestPath we clamp cosOmega to
// -1 to stay in the domain of Math.Acos below.
if (cosOmega < -1.0)
{
cosOmega = -1.0;
}
}
// Clamp cosOmega to [-1,1] to stay in the domain of Math.Acos below.
// The logic above has either flipped the sign of cosOmega to ensure it
// is positive or clamped to -1 aready. We only need to worry about the
// upper limit here.
if (cosOmega > 1.0)
{
cosOmega = 1.0;
}
Debug.Assert(!(cosOmega < -1.0) && !(cosOmega > 1.0),
"cosOmega should be clamped to [-1,1]");
// The mainline algorithm doesn't work for extreme
// cosine values. For large cosine we have a better
// fallback hence the asymmetric limits.
const double maxCosine = 1.0 - 1e-6;
const double minCosine = 1e-10 - 1.0;
// Calculate scaling coefficients.
if (cosOmega > maxCosine)
{
// Quaternions are too close - use linear interpolation.
scaleFrom = 1.0 - t;
scaleTo = t;
}
else if (cosOmega < minCosine)
{
// Quaternions are nearly opposite, so we will pretend to
// is exactly -from.
// First assign arbitrary perpendicular to "to".
to = new Quaternion(-from.Y, from.X, -from.W, from.Z);
double theta = t * Math.PI;
scaleFrom = Math.Cos(theta);
scaleTo = Math.Sin(theta);
}
else
{
// Standard case - use SLERP interpolation.
double omega = Math.Acos(cosOmega);
double sinOmega = Math.Sqrt(1.0 - cosOmega*cosOmega);
scaleFrom = Math.Sin((1.0 - t) * omega) / sinOmega;
scaleTo = Math.Sin(t * omega) / sinOmega;
}
// We want the magnitude of the output quaternion to be
// multiplicatively interpolated between the input
// magnitudes, i.e. lengthOut = lengthFrom * (lengthTo/lengthFrom)^t
// = lengthFrom ^ (1-t) * lengthTo ^ t
double lengthOut = lengthFrom * Math.Pow(lengthTo/lengthFrom, t);
scaleFrom *= lengthOut;
scaleTo *= lengthOut;
return new Quaternion(scaleFrom*from._x + scaleTo*to._x,
scaleFrom*from._y + scaleTo*to._y,
scaleFrom*from._z + scaleTo*to._z,
scaleFrom*from._w + scaleTo*to._w);
}
#endregion Public Methods
#region Private Methods
static private double Max(double a, double b, double c, double d)
{
if (b > a)
a = b;
if (c > a)
a = c;
if (d > a)
a = d;
return a;
}
#endregion Private Methods
//------------------------------------------------------
//
// Public Properties
//
//------------------------------------------------------
#region Public Properties
///
/// X - Default value is 0.
///
public double X
{
get
{
return _x;
}
set
{
if (IsDistinguishedIdentity)
{
this = s_identity;
IsDistinguishedIdentity = false;
}
_x = value;
}
}
///
/// Y - Default value is 0.
///
public double Y
{
get
{
return _y;
}
set
{
if (IsDistinguishedIdentity)
{
this = s_identity;
IsDistinguishedIdentity = false;
}
_y = value;
}
}
///
/// Z - Default value is 0.
///
public double Z
{
get
{
return _z;
}
set
{
if (IsDistinguishedIdentity)
{
this = s_identity;
IsDistinguishedIdentity = false;
}
_z = value;
}
}
///
/// W - Default value is 1.
///
public double W
{
get
{
if (IsDistinguishedIdentity)
{
return 1.0;
}
else
{
return _w;
}
}
set
{
if (IsDistinguishedIdentity)
{
this = s_identity;
IsDistinguishedIdentity = false;
}
_w = value;
}
}
#endregion Public Properties
//-----------------------------------------------------
//
// Internal Fields
//
//------------------------------------------------------
#region Internal Fields
internal double _x;
internal double _y;
internal double _z;
internal double _w;
#endregion Internal Fields
#region Private Fields and Properties
// If this bool is false then we are a default quaternion with
// all doubles equal to zero, but should be treated as
// identity.
private bool _isNotDistinguishedIdentity;
private bool IsDistinguishedIdentity
{
get
{
return !_isNotDistinguishedIdentity;
}
set
{
_isNotDistinguishedIdentity = !value;
}
}
private static int GetIdentityHashCode()
{
// This code is called only once.
double zero = 0;
double one = 1;
// return zero.GetHashCode() ^ zero.GetHashCode() ^ zero.GetHashCode() ^ one.GetHashCode();
// But this expression can be simplified because the first two hash codes cancel.
return zero.GetHashCode() ^ one.GetHashCode();
}
private static Quaternion GetIdentity()
{
// This code is called only once.
Quaternion q = new Quaternion(0,0,0,1);
q.IsDistinguishedIdentity = true;
return q;
}
// Hash code for identity.
private static int c_identityHashCode = GetIdentityHashCode();
// Default identity
private static Quaternion s_identity = GetIdentity();
#endregion Private Fields and Properties
}
}
// File provided for Reference Use Only by Microsoft Corporation (c) 2007.
// Copyright (c) Microsoft Corporation. All rights reserved.
Link Menu
This book is available now!
Buy at Amazon US or
Buy at Amazon UK
- UiaCoreApi.cs
- TrackingQueryElement.cs
- AutomationFocusChangedEventArgs.cs
- Merger.cs
- Rules.cs
- SqlConnectionPoolGroupProviderInfo.cs
- TypeUtils.cs
- ListenerHandler.cs
- RegexCompiler.cs
- StringComparer.cs
- TreeNodeSelectionProcessor.cs
- Message.cs
- XPathPatternBuilder.cs
- RSAPKCS1SignatureDeformatter.cs
- DataGridItemEventArgs.cs
- FrameworkContentElement.cs
- UInt64Storage.cs
- PrintControllerWithStatusDialog.cs
- DeleteWorkflowOwnerCommand.cs
- HtmlFormWrapper.cs
- SiteMapDataSource.cs
- VectorAnimationBase.cs
- DataObjectCopyingEventArgs.cs
- UICuesEvent.cs
- DbDataAdapter.cs
- ReadOnlyActivityGlyph.cs
- LicenseProviderAttribute.cs
- XmlUtf8RawTextWriter.cs
- NameValuePair.cs
- XMLDiffLoader.cs
- X500Name.cs
- XmlSchemaChoice.cs
- CalendarDay.cs
- DPTypeDescriptorContext.cs
- ApplicationFileCodeDomTreeGenerator.cs
- OraclePermission.cs
- UnsafeNetInfoNativeMethods.cs
- EntityRecordInfo.cs
- XmlBinaryReaderSession.cs
- IntegerValidatorAttribute.cs
- ArrangedElement.cs
- DataGridViewControlCollection.cs
- HttpModuleAction.cs
- RSACryptoServiceProvider.cs
- SecurityTokenSpecification.cs
- CodeArgumentReferenceExpression.cs
- ExpressionBinding.cs
- Encoder.cs
- WebEventTraceProvider.cs
- MimeTypePropertyAttribute.cs
- Point3DAnimationUsingKeyFrames.cs
- AssemblyBuilder.cs
- RealizationDrawingContextWalker.cs
- EndpointConfigContainer.cs
- SqlProviderServices.cs
- ControlValuePropertyAttribute.cs
- XmlSigningNodeWriter.cs
- CodeTypeOfExpression.cs
- TextContainerChangedEventArgs.cs
- FormatVersion.cs
- BaseParser.cs
- BindingMAnagerBase.cs
- TextWriter.cs
- AnnotationMap.cs
- ComponentChangingEvent.cs
- SqlCacheDependency.cs
- SrgsGrammar.cs
- ImageInfo.cs
- oledbmetadatacollectionnames.cs
- coordinatorfactory.cs
- Visual.cs
- ReadOnlyCollectionBuilder.cs
- LabelLiteral.cs
- EngineSiteSapi.cs
- AuthenticationModuleElementCollection.cs
- prefixendpointaddressmessagefilter.cs
- GridViewColumn.cs
- SplitterPanel.cs
- DataServiceProcessingPipelineEventArgs.cs
- DataControlImageButton.cs
- ItemsPresenter.cs
- ToolBarTray.cs
- CustomSignedXml.cs
- CallTemplateAction.cs
- StorageEntityContainerMapping.cs
- TextSpan.cs
- ExpressionList.cs
- XmlQueryType.cs
- SpotLight.cs
- DashStyle.cs
- FloaterBaseParagraph.cs
- ListViewCancelEventArgs.cs
- FileUtil.cs
- StrongNamePublicKeyBlob.cs
- WebPartMenu.cs
- FixedSOMLineCollection.cs
- ButtonBase.cs
- DataGridViewRowHeightInfoPushedEventArgs.cs
- SimpleHandlerFactory.cs
- InputEventArgs.cs