public static Quaternion rotatePointByAngleAboutAxisVector(Quaternion point, double angle, Quaternion axis)
{
Quaternion result = new Quaternion(0, 0, 0, 0);
// axis (r)
// axis must be a unit vector
axis.setX(axis.getX() / axis.magnitude());
axis.setY(axis.getY() / axis.magnitude());
axis.setZ(axis.getZ() / axis.magnitude());
// angle (a)
// dot product (dp) = cos(a)
double dp = Math.Cos(angle);
if (debugMode)
Console.WriteLine("dot product: " + dp);
// s = sqrt(2*(1+dp))
double s = Math.Sqrt(2 * (1 + dp));
if (debugMode)
Console.WriteLine("s: " + s);
// q
Quaternion q = new Quaternion(0, 0, 0, 0);
q.setW(s/2);
q.setX(axis.getX() / s);
q.setY(axis.getY() / s);
q.setZ(axis.getZ() / s);
if (debugMode)
Console.WriteLine("q: " + q);
// point (p)
// p'= qpq^-1
result = q.multiply(point);
result = result.multiply(q.inverse());
// final round for output (if set)
if (rotationRoundingForOutput)
{
result.setW(Math.Round(result.getW(), 2));
result.setX(Math.Round(result.getX(), 2));
result.setY(Math.Round(result.getY(), 2));
result.setZ(Math.Round(result.getZ(), 2));
}
return result;
}
public void runTests()
{
Quaternion quatA = new Quaternion(1,2,1,2);
Quaternion quatB = new Quaternion(1,-1,0,2);
Quaternion result = new Quaternion(0,0,0,0);
// test inverse
TLog.Text += "test inverse\n";
TLog.Text += quatA + "^-1\n";
result = quatA.inverse();
TLog.Text += "= " + result + "\n\n";
quatA = new Quaternion(3, 2, 1, 0);
// test addition
TLog.Text += "test add\n";
TLog.Text += quatA + " + " + quatB + "\n";
result = quatA.add(quatB);
TLog.Text += "= " + result + "\n\n";
// test subtraction
TLog.Text += "test subtract\n";
TLog.Text += quatA + " - " + quatB + "\n";
result = quatA.subtract(quatB);
TLog.Text += "= " + result + "\n\n";
quatB = new Quaternion(0,1,-2,0);
// test multiplication
TLog.Text += "test multiply\n";
TLog.Text += quatA + " * " + quatB + "\n";
result = quatA.multiply(quatB);
TLog.Text += "= " + result + "\n\n";
// test division
TLog.Text += "test divide\n";
TLog.Text += quatA + " / " + quatB + "\n";
result = quatA.divide(quatB);
TLog.Text += "inverse of quatB: " + quatB.inverse() + "\n";
TLog.Text += "= " + result + "\n\n";
quatA = new Quaternion(1,2,1,2);
// test magnitude
TLog.Text += "test magnitude\n";
TLog.Text += quatA + "\n";
TLog.Text += "= " + quatA.magnitude() + "\n\n";
// test rotation
Quaternion point = new Quaternion(0, 5, 0, 0);
Double angle = Math.PI / 2;
Quaternion axis = new Quaternion(0, 0, 0, 1);
TLog.Text += "test quaternion rotation\n";
TLog.Text += "point: " + point + "\n";
TLog.Text += "angle: " + angle + "\n";
TLog.Text += "axis: " + axis + "\n";
result = Quaternion.rotatePointByAngleAboutAxisVector(point, angle, axis);
TLog.Text += "= " + result + "\n\n";
// verify rotation manually
TLog.Text += "test quaternion rotation (manual functions check)\n";
quatA = new Quaternion(Math.Sqrt(2) / 2, 0, 0, Math.Sqrt(2) / 2);
Quaternion quatP = new Quaternion(0, 5, 0, 0);
TLog.Text += "-- inverse\n";
TLog.Text += quatA + "^-1\n";
result = quatA.inverse();
TLog.Text += result + "\n\n";
TLog.Text += "-- multiply\n";
TLog.Text += quatA + " * " + quatP + "\n\n";
result = quatA.multiply(quatP);
result = result.multiply(quatA.inverse());
TLog.Text += "= " + result + "\n\n";
}
public static void Main(string[] args)
{
Quaternion quatA = new Quaternion(1,2,1,2);
Quaternion quatB = new Quaternion(1,-1,0,2);
Quaternion result = new Quaternion(0,0,0,0);
// test inverse
Console.WriteLine("inverse");
Console.WriteLine(quatA + "^-1");
result = quatA.inverse();
Console.WriteLine(result + "\n");
quatA = new Quaternion(3, 2, 1, 0);
// test addition
Console.WriteLine("add");
Console.WriteLine(quatA + " + " + quatB);
result = quatA.add(quatB);
Console.WriteLine(result + "\n");
// test subtraction
Console.WriteLine("subtract");
Console.WriteLine(quatA + " - " + quatB);
result = quatA.subtract(quatB);
Console.WriteLine(result + "\n");
quatB = new Quaternion(0,1,-2,0);
// test multiplication
Console.WriteLine("multiply");
Console.WriteLine(quatA + " * " + quatB);
result = quatA.multiply(quatB);
Console.WriteLine(result + "\n");
// test division
// #ADDME
Console.WriteLine("divide");
Console.WriteLine(quatA + " / " + quatB);
result = quatA.divide(quatB);
Console.WriteLine("inverse of quatB: " + quatB.inverse());
Console.WriteLine(result + "\n");
quatA = new Quaternion(1,2,1,2);
// test magnitude
Console.WriteLine("magnitude");
Console.WriteLine(quatA);
Console.WriteLine(quatA.magnitude() + "\n");
// test rotation
Quaternion axis = new Quaternion(0, 0, 0, 1);
Quaternion point = new Quaternion(0, 5, 0, 0);
Console.WriteLine("quaternion rotation");
result = Quaternion.rotatePointByAngleAboutAxisVector(point, Math.PI/2, axis);
Console.WriteLine(result + "\n");
// verify rotation manually
Console.WriteLine("quaternion rotation manual check");
quatA = new Quaternion(Math.Sqrt(2) / 2, 0, 0, Math.Sqrt(2) / 2);
Quaternion quatP = new Quaternion(0, 5, 0, 0);
Console.WriteLine("-- inverse");
Console.WriteLine(quatA + "^-1");
result = quatA.inverse();
Console.WriteLine(result + "\n");
Console.WriteLine("-- multiply");
Console.WriteLine(quatA + " * " + quatP);
result = quatA.multiply(quatP);
result = result.multiply(quatA.inverse());
Console.WriteLine(result + "\n");
}
private void Operation(String op)
{
Quaternion quatA = new Quaternion(0,0,0,0);
Quaternion quatB = new Quaternion(0,0,0,0);
Quaternion quatResult = new Quaternion(0,0,0,0);
Double doubleResult = 0.0f;
switch (op)
{
case "add":
quatA = parseStringToQuat(OPQuatABox.Text);
quatB = parseStringToQuat(OPQuatBBox.Text);
quatResult = quatA.add(quatB);
OPResultBox.Text = quatResult.ToString();
break;
case "subtract":
quatA = parseStringToQuat(OPQuatABox.Text);
quatB = parseStringToQuat(OPQuatBBox.Text);
quatResult = quatA.subtract(quatB);
OPResultBox.Text = quatResult.ToString();
break;
case "multiply":
quatA = parseStringToQuat(OPQuatABox.Text);
quatB = parseStringToQuat(OPQuatBBox.Text);
quatResult = quatA.multiply(quatB);
OPResultBox.Text = quatResult.ToString();
break;
case "divide":
quatA = parseStringToQuat(OPQuatABox.Text);
quatB = parseStringToQuat(OPQuatBBox.Text);
quatResult = quatA.divide(quatB);
OPResultBox.Text = quatResult.ToString();
break;
case "inverse":
quatA = parseStringToQuat(IMQuatBox.Text);
quatResult = quatA.inverse();
IMResultBox.Text = quatResult.ToString();
break;
case "magnitude":
quatA = parseStringToQuat(IMQuatBox.Text);
doubleResult = quatA.magnitude();
IMResultBox.Text = doubleResult.ToString();
break;
case "rotate":
quatA = parseStringToQuat(RQuatABox.Text);
quatB = parseStringToQuat(RQuatBBox.Text);
double angleNumerator = 1.0f;
if (!string.IsNullOrWhiteSpace(RAngleNumerator.Text))
angleNumerator = Convert.ToDouble(RAngleNumerator.Text);
double piInNumerator = 1.0f;
if (RAngleIncludePIInNumerator.IsChecked.Value)
piInNumerator = Math.PI;
double angleDenominator = Convert.ToDouble(RAngleDenominator.Text);
double angle = (angleNumerator * piInNumerator) / angleDenominator;
quatResult = Quaternion.rotatePointByAngleAboutAxisVector(quatA, angle, quatB);
RResultBox.Text = quatResult.ToString();
break;
default:
break;
}
}
