public void RotateAt(Point3D pt, Quaternion q)
{
foreach (var cuboid in cuboids)
{
cuboid.RotateAt(pt, q);
}
}
protected override void OnLoad(EventArgs e)
{
base.OnLoad(e);
// buffer = new Bitmap(Width, Height);
cub.Center = new Point3D(400, 240, 0);
cam.Location = new Point3D(400, 240, -500);
ReDraw();
timer = new Timer();
timer.Interval = 20;
timer.Tick += (s, ee) =>
{
//var rand = new Random(DateTime.Now.Millisecond);
//var x = 10;// rand.Next(10);
//var y = 10;// rand.Next(10);
//var z = 10;// rand.Next(10);
int x = 0, y = 0, z = 0;
switch (count++ % 30 / 10)
{
case 0:
cubeX += 10;
labelCrX.Text = cubeX.ToString();
x = 1;
break;
case 1:
cubeZ += 10;
labelCrZ.Text = cubeZ.ToString();
y = 1;
break;
case 2:
cubeY += 10;
labelCrY.Text = cubeY.ToString();
z = 1;
break;
default:
break;
}
//var q = new Quaternion(new Vector3D(1, 0, 0), x ) *
// new Quaternion(new Vector3D(0, 0, 1), y ) *
// new Quaternion(new Vector3D(0, 1, 0), z );
var q = new Quaternion(new Vector3D(x, y, z), 10);
cub.RotateAt(cub.Center, q);
ReDraw();
};
}
protected override void OnLoad(EventArgs e)
{
base.OnLoad(e);
// buffer = new Bitmap(Width, Height);
cub.Center = new Point3D(400, 400, 0);
cam.Location = new Point3D(400, 400, -1000);
timer = new Timer();
timer.Interval = 50;
timer.Tick += (s, ee) =>
{
var index = count++ % 30 / 10;
var q = new Quaternion(new Vector3D(index == 0 ? 1 : 0, index == 1 ? 1 : 0, index == 2 ? 1 : 0), 10);
cub.RotateAt(cub.Center, q);
Invalidate();
};
timer.Start();
}
/// <summary>
/// Equals - compares this Quaternion with the passed in object. In this equality
/// Double.NaN is equal to itself, unlike in numeric equality.
/// Note that double values can acquire error when operated upon, such that
/// an exact comparison between two values which
/// are logically equal may fail.
/// </summary>
/// <returns>
/// bool - true if "value" is equal to "this".
/// </returns>
/// <param name='value'>The Quaternion to compare to "this"</param>
public bool Equals(Quaternion value)
{
return Quaternion.Equals(this, value);
}
private static Quaternion GetIdentity()
{
// This code is called only once.
Quaternion q = new Quaternion(0, 0, 0, 1);
q.IsDistinguishedIdentity = true;
return q;
}
/// <summary>
/// Compares two Quaternion instances for object equality. In this equality
/// Double.NaN is equal to itself, unlike in numeric equality.
/// Note that double values can acquire error when operated upon, such that
/// an exact comparison between two values which
/// are logically equal may fail.
/// </summary>
/// <returns>
/// bool - true if the two Quaternion instances are exactly equal, false otherwise
/// </returns>
/// <param name='quaternion1'>The first Quaternion to compare</param>
/// <param name='quaternion2'>The second Quaternion to compare</param>
public static bool Equals(Quaternion quaternion1, Quaternion quaternion2)
{
if (quaternion1.IsDistinguishedIdentity || quaternion2.IsDistinguishedIdentity)
{
return quaternion1.IsIdentity == quaternion2.IsIdentity;
}
else
{
return quaternion1.X.Equals(quaternion2.X) &&
quaternion1.Y.Equals(quaternion2.Y) &&
quaternion1.Z.Equals(quaternion2.Z) &&
quaternion1.W.Equals(quaternion2.W);
}
}
/// <summary>
/// Smoothly interpolate between the two given quaternions using Spherical
/// Linear Interpolation (SLERP).
/// </summary>
/// <param name="from">First quaternion for interpolation.</param>
/// <param name="to">Second quaternion for interpolation.</param>
/// <param name="t">Interpolation coefficient.</param>
/// <returns>SLERP-interpolated quaternion between the two given quaternions.</returns>
public static Quaternion Slerp(Quaternion from, Quaternion to, double t)
{
return Slerp(from, to, t, /* useShortestPath = */ true);
}
/// <summary>
/// Smoothly interpolate between the two given quaternions using Spherical
/// Linear Interpolation (SLERP).
/// </summary>
/// <param name="from">First quaternion for interpolation.</param>
/// <param name="to">Second quaternion for interpolation.</param>
/// <param name="t">Interpolation coefficient.</param>
/// <param name="useShortestPath">If true, Slerp will automatically flip the sign of
/// the destination Quaternion to ensure the shortest path is taken.</param>
/// <returns>SLERP-interpolated quaternion between the two given quaternions.</returns>
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);
}
/// <summary>
/// Quaternion multiplication.
/// </summary>
/// <param name="left">First quaternion.</param>
/// <param name="right">Second quaternion.</param>
/// <returns>Result of multiplication.</returns>
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;
}
/// <summary>
/// Quaternion multiplication.
/// </summary>
/// <param name="left">First quaternion.</param>
/// <param name="right">Second quaternion.</param>
/// <returns>Result of multiplication.</returns>
public static Quaternion Multiply(Quaternion left, Quaternion right)
{
return left * right;
}
/// <summary>
/// Quaternion addition.
/// </summary>
/// <param name="left">First quaternion being added.</param>
/// <param name="right">Second quaternion being added.</param>
/// <returns>Result of addition.</returns>
public static Quaternion Add(Quaternion left, Quaternion right)
{
return (left + right);
}
/// <summary>
/// Quaternion subtraction.
/// </summary>
/// <param name="left">Quaternion to subtract from.</param>
/// <param name="right">Quaternion to subtract from the first quaternion.</param>
/// <returns>Result of subtraction.</returns>
public static Quaternion Subtract(Quaternion left, Quaternion right)
{
return (left - right);
}
// Creates a rotation matrix given a quaternion and center.
//
// Quaternion and center are passed by reference for performance
// only and are not modified.
//
internal static Matrix3D CreateRotationMatrix(ref Quaternion quaternion, ref Point3D center)
{
Matrix3D matrix = s_identity;
matrix.IsDistinguishedIdentity = false; // Will be using direct member access
double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
x2 = quaternion.X + quaternion.X;
y2 = quaternion.Y + quaternion.Y;
z2 = quaternion.Z + quaternion.Z;
xx = quaternion.X * x2;
xy = quaternion.X * y2;
xz = quaternion.X * z2;
yy = quaternion.Y * y2;
yz = quaternion.Y * z2;
zz = quaternion.Z * z2;
wx = quaternion.W * x2;
wy = quaternion.W * y2;
wz = quaternion.W * z2;
matrix._m11 = 1.0 - (yy + zz);
matrix._m12 = xy + wz;
matrix._m13 = xz - wy;
matrix._m21 = xy - wz;
matrix._m22 = 1.0 - (xx + zz);
matrix._m23 = yz + wx;
matrix._m31 = xz + wy;
matrix._m32 = yz - wx;
matrix._m33 = 1.0 - (xx + yy);
if (center.X != 0 || center.Y != 0 || center.Z != 0)
{
matrix._offsetX = -center.X * matrix._m11 - center.Y * matrix._m21 - center.Z * matrix._m31 + center.X;
matrix._offsetY = -center.X * matrix._m12 - center.Y * matrix._m22 - center.Z * matrix._m32 + center.Y;
matrix._offsetZ = -center.X * matrix._m13 - center.Y * matrix._m23 - center.Z * matrix._m33 + center.Z;
}
return matrix;
}
private void button10_Click(object sender, EventArgs e)
{
cubeY += 5;
labelCrY.Text = cubeY.ToString();
Quaternion q = new Quaternion(new Vector3D(0, 1, 0), 5);
cub.RotateAt(cub.Center, q);
ReDraw();
}
/// <summary>
/// Prepends rotation transform to the current matrix.
/// </summary>
/// <param name="quaternion">Quaternion representing rotation.</param>
/// <param name="center">Center to rotate around.</param>
public void RotateAtPrepend(Quaternion quaternion, Point3D center)
{
this = CreateRotationMatrix(ref quaternion, ref center) * this;
}
/// <summary>
/// Appends rotation transform to the current matrix.
/// </summary>
/// <param name="quaternion">Quaternion representing rotation.</param>
/// <param name="center">Center to rotate around.</param>
public void RotateAt(Quaternion quaternion, Point3D center)
{
this *= CreateRotationMatrix(ref quaternion, ref center);
}
/// <summary>
/// Prepends rotation transform to the current matrix.
/// </summary>
/// <param name="quaternion">Quaternion representing rotation.</param>
public void RotatePrepend(Quaternion quaternion)
{
Point3D center = new Point3D();
this = CreateRotationMatrix(ref quaternion, ref center) * this;
}
//------------------------------------------------------
//
// Rotate
//
//------------------------------------------------------
/// <summary>
/// Appends rotation transform to the current matrix.
/// </summary>
/// <param name="quaternion">Quaternion representing rotation.</param>
public void Rotate(Quaternion quaternion)
{
Point3D center = new Point3D();
this *= CreateRotationMatrix(ref quaternion, ref center);
}
private void button7_Click(object sender, EventArgs e)
{
cubeZ -= 5;
labelCrZ.Text = cubeZ.ToString();
Quaternion q = new Quaternion(new Vector3D(0, 0, 1), -5);
cub.RotateAt(cub.Center, q);
ReDraw();
}
