/// <summary>
/// This function creates a vector that is the vector passed in rotated by my definition
/// </summary>
/// <param name="vector">The vector to rotate (I don't touch this vector, I create a new one that is rotated)</param>
/// <param name="isQuatNormalized">Whether this class is already normalized or not (if you don't know, pass false)</param>
public MyVector GetRotatedVector(MyVector vector, bool isQuatNormalized)
{
if (!isQuatNormalized)
{
// I'm not normalized, clone myself and normalize it
MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
myUnitClone.BecomeUnitQuaternion();
return(myUnitClone.GetRotatedVector(vector, true));
}
MyVector qvec = new MyVector(this.X, this.Y, this.Z);
//Vector uv = qvec.Cross(vector);
MyVector uv = MyVector.Cross(qvec, vector);
//Vector uuv = qvec.Cross(uv);
MyVector uuv = MyVector.Cross(qvec, uv);
//uv *= (2.0f * quat.w);
uv.Multiply(this.W * 2d);
//uuv *= 2.0f;
uuv.Multiply(2d);
//return vector + uv + uuv;
MyVector retVal = vector.Clone();
retVal.Add(uv);
retVal.Add(uuv);
return(retVal);
}
public DoubleVector GetRotatedVector(DoubleVector doubleVector, bool isQuatNormalized)
{
if (!isQuatNormalized)
{
// I'm not normalized, clone myself and normalize it
MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
myUnitClone.BecomeUnitQuaternion();
return(myUnitClone.GetRotatedVector(doubleVector, true));
}
return(new DoubleVector(GetRotatedVector(doubleVector.Standard, true), GetRotatedVector(doubleVector.Orth, true)));
}
/// <summary>
/// This function will rotate me around any arbitrary axis (no gimbal lock for me!!!!!!!)
/// </summary>
/// <remarks>
/// From some web site talking about rotations (seems like good stuff, but I don't know how it's used):
///
/// // I'm not sure why this multiplication is important
/// Quaternion totalQuat = Quaternion.MultiplyQuaternions(rotationQuat, new Quaternion(1, 0, 0, 0)); // order of multiplication is important
///
/// // Get a matrix telling me how to rotate
/// Matrix rotationMatrix = totalQuat.ToMatrixFromUnitQuaternion();
///
/// // AND THEN??????????
/// </remarks>
/// <param name="rotateAround">Any vector to rotate around</param>
/// <param name="radians">How far to rotate</param>
public void RotateAroundAxis(MyVector rotateAround, double radians)
{
// Create a quaternion that represents the axis and angle passed in
MyQuaternion rotationQuat = new MyQuaternion(rotateAround, radians);
// Get a vector that represents me rotated by the quaternion
MyVector newValue = rotationQuat.GetRotatedVector(this, true);
// Store my new values
this.X = newValue.X;
this.Y = newValue.Y;
this.Z = newValue.Z;
}
/// <summary>
/// This adds external torque to internal torque. Before I do that, I need to rotate a copy of external torque
/// so that it is the same orientation as the internal torque.
/// </summary>
private void CombineExternalAndInternalTorques()
{
// My base's rotation quaternion already knows how to rotate from orig to dirfacing. I need one that
// goes the other way
MyQuaternion rotation = base.Rotation.Clone();
rotation.W *= -1;
// Rotate the external to line up with the internal
MyVector externalRotated = rotation.GetRotatedVector(_externalTorque, true);
// Now that it's aligned, I can just add it onto the internal
_internalTorque.Add(externalRotated);
}
/// <summary>
/// Since the quaternion doesn't have any events, this function needs to be called manually whenever
/// a rotation occurs
/// </summary>
/// <remarks>
/// I need to take in the rotation, because the Sphere class keeps blowing away it's rotation, so this would
/// immediately fall out of sync
/// </remarks>
public void SyncRotation(MyQuaternion rotation)
{
// I don't know if there's much of a gain in caching (or even a loss)
MyVector[] cachedBasePoints = base.UniquePoints;
MyVector[] cachedThisPoints = _rotatedPoly.UniquePoints;
// Rotate all the points
for (int pointCntr = 0; pointCntr < cachedBasePoints.Length; pointCntr++)
{
cachedThisPoints[pointCntr].StoreNewValues(rotation.GetRotatedVector(cachedBasePoints[pointCntr], true));
}
// Remember this for later
_rotationForClone = rotation;
}
/// <summary>
/// This function takes in a destination double vector, and I will tell you how much you need to rotate me in order for me to end up
/// along that destination double vector.
/// </summary>
/// <remarks>
/// This function is a mutated copy of MyVector.GetAngleBetweenVectors. It is almost identical, but slightly more complex :)
///
/// If I am already aligned with the vector passed in, then I will return an arbitrary orthoganal, and an angle of zero.
/// </remarks>
/// <param name="destination">This is the double vector you want me to align myself with</param>
public MyQuaternion GetAngleAroundAxis(DoubleVector destination)
{
#region Standard
// Get the angle
double rotationRadians = MyVector.GetAngleBetweenVectors(this.Standard, destination.Standard);
if (Double.IsNaN(rotationRadians))
{
rotationRadians = 0d;
}
// I need to pull the cross product from me to the vector passed in
MyVector rotationAxis = MyVector.Cross(this.Standard, destination.Standard);
// If the cross product is zero, then there are two possibilities. The vectors point in the same direction, or opposite directions.
if (rotationAxis.IsNearZero)
{
// If I am here, then the angle will either be 0 or PI.
if (Utility3D.IsNearZero(rotationRadians))
{
// The vectors sit on top of each other. I will set the orthoganal to an arbitrary value, and return zero for the radians
rotationAxis.X = 1d;
rotationAxis.Y = 0d;
rotationAxis.Z = 0d;
rotationRadians = 0d;
}
else
{
// The vectors are pointing directly away from each other, because this is a double vector, I must rotate along an axis that
// is orthogonal to my standard and orth
rotationAxis = this.Orth; //MyVector.Cross(this.Standard, this.Orth);
}
}
MyQuaternion quatStandard = new MyQuaternion(rotationAxis, rotationRadians);
//return quatStandard;
#endregion
// I only need to rotate the orth, because I already know where the standard will be
MyVector rotatedOrth = quatStandard.GetRotatedVector(this.Orth, true);
#region Orthogonal
// Grab the angle
rotationRadians = MyVector.GetAngleBetweenVectors(rotatedOrth, destination.Orth);
if (Double.IsNaN(rotationRadians))
{
rotationRadians = 0d;
}
// Since I've rotated the standards onto each other, the rotation axis of the orth is the standard (asumming it was truely orthogonal
// to begin with)
rotationAxis = destination.Standard.Clone();
MyQuaternion quatOrth = new MyQuaternion(rotationAxis, rotationRadians);
#endregion
// Exit Function
//return MyQuaternion.Multiply(quatOrth, quatStandard);
return MyQuaternion.Multiply(quatStandard, quatOrth);
}
/// <summary>
/// This function will rotate me around any arbitrary axis (no gimbal lock for me!!!!!!!)
/// </summary>
/// <remarks>
/// From some web site talking about rotations (seems like good stuff, but I don't know how it's used):
///
/// // I'm not sure why this multiplication is important
/// Quaternion totalQuat = Quaternion.MultiplyQuaternions(rotationQuat, new Quaternion(1, 0, 0, 0)); // order of multiplication is important
///
/// // Get a matrix telling me how to rotate
/// Matrix rotationMatrix = totalQuat.ToMatrixFromUnitQuaternion();
///
/// // AND THEN??????????
/// </remarks>
/// <param name="rotateAround">Any vector to rotate around</param>
/// <param name="radians">How far to rotate</param>
public void RotateAroundAxis(MyVector rotateAround, double radians)
{
// Create a quaternion that represents the axis and angle passed in
MyQuaternion rotationQuat = new MyQuaternion(rotateAround, radians);
// Get a vector that represents me rotated by the quaternion
MyVector newValue = rotationQuat.GetRotatedVector(this, true);
// Store my new values
this.X = newValue.X;
this.Y = newValue.Y;
this.Z = newValue.Z;
}
/// <summary>
/// This function creates a vector that is the vector passed in rotated by my definition
/// </summary>
/// <param name="vector">The vector to rotate (I don't touch this vector, I create a new one that is rotated)</param>
/// <param name="isQuatNormalized">Whether this class is already normalized or not (if you don't know, pass false)</param>
public MyVector GetRotatedVector(MyVector vector, bool isQuatNormalized)
{
if (!isQuatNormalized)
{
// I'm not normalized, clone myself and normalize it
MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
myUnitClone.BecomeUnitQuaternion();
return myUnitClone.GetRotatedVector(vector, true);
}
MyVector qvec = new MyVector(this.X, this.Y, this.Z);
//Vector uv = qvec.Cross(vector);
MyVector uv = MyVector.Cross(qvec, vector);
//Vector uuv = qvec.Cross(uv);
MyVector uuv = MyVector.Cross(qvec, uv);
//uv *= (2.0f * quat.w);
uv.Multiply(this.W * 2d);
//uuv *= 2.0f;
uuv.Multiply(2d);
//return vector + uv + uuv;
MyVector retVal = vector.Clone();
retVal.Add(uv);
retVal.Add(uuv);
return retVal;
}
public DoubleVector GetRotatedVector(DoubleVector doubleVector, bool isQuatNormalized)
{
if (!isQuatNormalized)
{
// I'm not normalized, clone myself and normalize it
MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
myUnitClone.BecomeUnitQuaternion();
return myUnitClone.GetRotatedVector(doubleVector, true);
}
return new DoubleVector(GetRotatedVector(doubleVector.Standard, true), GetRotatedVector(doubleVector.Orth, true));
}
private void button3_Click(object sender, EventArgs e)
{
ClearPictureBox();
// Setup Orig Vector
MyVector origVector = new MyVector(9, 0, 0);
DrawVector(origVector, Color.Silver);
#region Single Rotation
// Setup quat to do the whole rotation in one shot
MyQuaternion largeRotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(90));
MyVector largeRotation = largeRotationQuat.GetRotatedVector(origVector, true);
DrawVector(largeRotation, Color.White);
#endregion
#region Multi Rotations
// Setup quat that holds a 30 degree angle
MyQuaternion multiRotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(30));
MyVector subRotation = multiRotationQuat.GetRotatedVector(origVector, true);
DrawVector(subRotation, Color.Orange);
// Setup a quat that holds a 1 degree angle
MyQuaternion anotherRotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(1));
// Apply this to the multi quat 60 times
for (int cntr = 1; cntr <= 60; cntr++)
{
multiRotationQuat = MyQuaternion.Multiply(anotherRotationQuat, multiRotationQuat);
}
// Let's see what happened
subRotation = multiRotationQuat.GetRotatedVector(origVector, true);
DrawVector(subRotation, Color.HotPink);
#endregion
#region Multi Rotations (3D)
/*
// Rotate around Y
multiRotationQuat = new MyQuaternion(new MyVector(0, 1, 0), Utility3D.GetDegreesToRadians(90));
// Rotate around X
anotherRotationQuat = new MyQuaternion(new MyVector(1, 0, 0), Utility3D.GetDegreesToRadians(90));
multiRotationQuat = MyQuaternion.Multiply(anotherRotationQuat, multiRotationQuat);
// Draw the final output
subRotation = multiRotationQuat.GetRotatedVector(origVector, true);
DrawVector(subRotation, Color.Yellow);
*/
#endregion
#region Multi Rotations (Vector3D)
/*
subRotation = origVector.Clone();
subRotation.RotateAroundAxis(new MyVector(0, 1, 0), Utility3D.GetDegreesToRadians(90));
subRotation.RotateAroundAxis(new MyVector(1, 0, 0), Utility3D.GetDegreesToRadians(90));
DrawVector(subRotation, Color.Yellow);
*/
#endregion
}
private void button5_Click(object sender, EventArgs e)
{
// This button tests TorqueBall.OrthonormalizeOrientation
ClearPictureBox();
// Setup Orig Vector
MyVector origVector = new MyVector(9, 0, 0);
DrawVector(origVector, Color.Silver);
// Rotate around Z
MyQuaternion rotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(30));
MyVector rotated = rotationQuat.GetRotatedVector(origVector, true);
DrawVector(rotated, Color.Black);
MyMatrix3 rotationMatrix = rotationQuat.ToMatrix3FromUnitQuaternion();
// See if this affects the rotation matrix
TorqueBall.OrthonormalizeOrientation(rotationMatrix);
rotationQuat = null;
rotationQuat = new MyQuaternion();
rotationQuat.FromRotationMatrix(rotationMatrix);
rotationMatrix = null;
// Draw the results
rotated = rotationQuat.GetRotatedVector(origVector, true);
DrawVector(rotated, Color.DodgerBlue);
}
private void btnRotationMatrix_Click(object sender, EventArgs e)
{
ClearPictureBox();
// Setup Orig Vector
MyVector origVector = new MyVector(9, 0, 0);
DrawVector(origVector, Color.Silver);
// Rotate around Z
MyQuaternion rotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(30));
MyVector rotated = rotationQuat.GetRotatedVector(origVector, true);
DrawVector(rotated, Color.Black);
MyMatrix3 rotationMatrix;
for (int cntr = 1; cntr <= 10000000; cntr++)
{
rotationMatrix = rotationQuat.ToMatrix3FromUnitQuaternion();
rotationQuat = null;
rotationQuat = new MyQuaternion();
rotationQuat.FromRotationMatrix(rotationMatrix);
rotationMatrix = null;
}
rotated = rotationQuat.GetRotatedVector(origVector, true);
DrawVector(rotated, Color.DodgerBlue);
rotationQuat.W *= -1;
MyVector rotatedNegated = rotationQuat.GetRotatedVector(origVector, true);
DrawVector(rotatedNegated, Color.Yellow);
}
private void button4_Click(object sender, EventArgs e)
{
ClearPictureBox();
// Setup Orig Vector
MyVector origVector = new MyVector(9, 0, 0);
DrawVector(origVector, Color.Silver);
MyQuaternion multiRotationQuat = new MyQuaternion(new MyVector(0, 0, 0), Utility3D.GetDegreesToRadians(0));
// Rotate around Z
MyQuaternion anotherRotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(1));
//List<double> lengths = new List<double>();
for (int outerCntr = 1; outerCntr <= 100000; outerCntr++)
{
//lengths.Add(multiRotationQuat.GetMagnitude());
for (int innerCntr = 1; innerCntr <= 360; innerCntr++)
{
multiRotationQuat = MyQuaternion.Multiply(anotherRotationQuat, multiRotationQuat);
}
//multiRotationQuat.BecomeUnitQuaternion();
}
// Draw the final output
MyVector subRotation = multiRotationQuat.GetRotatedVector(origVector, true);
DrawVector(subRotation, Color.Yellow);
}
private void RotateMe(MyVector localPos, MyVector rotateHandle, MyQuaternion rotation2D, MyVector rotateAround3D)
{
// Get Current Angle
double curRadians = MyVector.GetAngleBetweenVectors(rotateHandle, rotation2D.GetRotatedVector(rotateHandle, true));
// Rotate the 2D Handle
MyQuaternion newRotationX;
rotateHandle.GetAngleAroundAxis(out newRotationX, localPos);
rotation2D.StoreNewValues(newRotationX);
// Get New Angle
double newRadians = MyVector.GetAngleBetweenVectors(rotateHandle, rotation2D.GetRotatedVector(rotateHandle, true));
double rotateRadians = newRadians - curRadians;
if (rotation2D.GetRotatedVector(rotateHandle, true).Y < 0) rotateRadians *= -1; // This statement is cheating. I'm using the fact that the rotate handles all lie on the XY plane
// Apply the difference to the 3D object (me)
this.RotateAroundAxis(this.Rotation.GetRotatedVector(rotateAround3D, true), rotateRadians);
}
/// <summary>
/// This function takes in a destination double vector, and I will tell you how much you need to rotate me in order for me to end up
/// along that destination double vector.
/// </summary>
/// <remarks>
/// This function is a mutated copy of MyVector.GetAngleBetweenVectors. It is almost identical, but slightly more complex :)
///
/// If I am already aligned with the vector passed in, then I will return an arbitrary orthoganal, and an angle of zero.
/// </remarks>
/// <param name="destination">This is the double vector you want me to align myself with</param>
public MyQuaternion GetAngleAroundAxis(DoubleVector destination)
{
#region Standard
// Get the angle
double rotationRadians = MyVector.GetAngleBetweenVectors(this.Standard, destination.Standard);
if (Double.IsNaN(rotationRadians))
{
rotationRadians = 0d;
}
// I need to pull the cross product from me to the vector passed in
MyVector rotationAxis = MyVector.Cross(this.Standard, destination.Standard);
// If the cross product is zero, then there are two possibilities. The vectors point in the same direction, or opposite directions.
if (rotationAxis.IsNearZero)
{
// If I am here, then the angle will either be 0 or PI.
if (Utility3D.IsNearZero(rotationRadians))
{
// The vectors sit on top of each other. I will set the orthoganal to an arbitrary value, and return zero for the radians
rotationAxis.X = 1d;
rotationAxis.Y = 0d;
rotationAxis.Z = 0d;
rotationRadians = 0d;
}
else
{
// The vectors are pointing directly away from each other, because this is a double vector, I must rotate along an axis that
// is orthogonal to my standard and orth
rotationAxis = this.Orth; //MyVector.Cross(this.Standard, this.Orth);
}
}
MyQuaternion quatStandard = new MyQuaternion(rotationAxis, rotationRadians);
//return quatStandard;
#endregion
// I only need to rotate the orth, because I already know where the standard will be
MyVector rotatedOrth = quatStandard.GetRotatedVector(this.Orth, true);
#region Orthogonal
// Grab the angle
rotationRadians = MyVector.GetAngleBetweenVectors(rotatedOrth, destination.Orth);
if (Double.IsNaN(rotationRadians))
{
rotationRadians = 0d;
}
// Since I've rotated the standards onto each other, the rotation axis of the orth is the standard (asumming it was truely orthogonal
// to begin with)
rotationAxis = destination.Standard.Clone();
MyQuaternion quatOrth = new MyQuaternion(rotationAxis, rotationRadians);
#endregion
// Exit Function
//return MyQuaternion.Multiply(quatOrth, quatStandard);
return(MyQuaternion.Multiply(quatStandard, quatOrth));
}
private void SyncDirFacing()
{
_dirFacing.Standard.StoreNewValues(_rotation.GetRotatedVector(_origDirFacing.Standard, true));
_dirFacing.Orth.StoreNewValues(_rotation.GetRotatedVector(_origDirFacing.Orth, true));
}
/// <summary>
/// Since the quaternion doesn't have any events, this function needs to be called manually whenever
/// a rotation occurs
/// </summary>
/// <remarks>
/// I need to take in the rotation, because the Sphere class keeps blowing away it's rotation, so this would
/// immediately fall out of sync
/// </remarks>
public void SyncRotation(MyQuaternion rotation)
{
// I don't know if there's much of a gain in caching (or even a loss)
MyVector[] cachedBasePoints = base.UniquePoints;
MyVector[] cachedThisPoints = _rotatedPoly.UniquePoints;
// Rotate all the points
for (int pointCntr = 0; pointCntr < cachedBasePoints.Length; pointCntr++)
{
cachedThisPoints[pointCntr].StoreNewValues(rotation.GetRotatedVector(cachedBasePoints[pointCntr], true));
}
// Remember this for later
_rotationForClone = rotation;
}
