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 Quaternion add(Quaternion other)
{
// (a + e)+i(b + f)+j(c + g)+k(d + h)
Quaternion result = new Quaternion(0, 0, 0, 0);
result.setW(this.getW() + other.getW());
result.setX(this.getX() + other.getX());
result.setY(this.getY() + other.getY());
result.setZ(this.getZ() + other.getZ());
return result;
}
public Quaternion subtract(Quaternion other)
{
// (a - e)+i(b - f)+j(c - g)+k(d - h)
Quaternion result = new Quaternion(0, 0, 0, 0);
result.setW(this.getW() - other.getW());
result.setX(this.getX() - other.getX());
result.setY(this.getY() - other.getY());
result.setZ(this.getZ() - other.getZ());
return result;
}
public Quaternion inverse()
{
double s = this.getW();
Dictionary<string, double> v = new Dictionary<string, double>();
Quaternion result = new Quaternion(0,0,0,0);
v.Add("x", this.getX());
v.Add("y", this.getY());
v.Add("z", this.getZ());
double totalToDivideBy = Math.Pow(s, 2)
+ Math.Pow(v["x"], 2)
+ Math.Pow(v["y"], 2)
+ Math.Pow(v["z"], 2);
result.setW(s / totalToDivideBy);
result.setX(-v["x"] / totalToDivideBy);
result.setY(-v["y"] / totalToDivideBy);
result.setZ(-v["z"] / totalToDivideBy);
return result;
}
public Quaternion multiply(Quaternion other)
{
Quaternion result = new Quaternion(0,0,0,0);
// FOIL
Pair myW = new Pair(this.getW(), 0);
Pair myX = new Pair(this.getX(), 1);
Pair myY = new Pair(this.getY(), 2);
Pair myZ = new Pair(this.getZ(), 3);
Pair otherW = new Pair(other.getW(), 0);
Pair otherX = new Pair(other.getX(), 1);
Pair otherY = new Pair(other.getY(), 2);
Pair otherZ = new Pair(other.getZ(), 3);
Dictionary<string, double> expression = new Dictionary<string, double>();
// type, value
expression.Add("w", 0);
expression.Add("i", 0);
expression.Add("j", 0);
expression.Add("k", 0);
expression.Add("ij", 0);
expression.Add("jk", 0);
expression.Add("ki", 0);
expression.Add("ji", 0);
expression.Add("kj", 0);
expression.Add("ik", 0);
expression.Add("i^2", 0);
expression.Add("j^2", 0);
expression.Add("k^2", 0);
// first group
expression["w"] += myW.getX() * otherW.getX();
expression["i"] += myW.getX() * otherX.getX();
expression["j"] += myW.getX() * otherY.getX();
expression["k"] += myW.getX() * otherZ.getX();
// second group
expression["i"] += myX.getX() * otherW.getX();
expression["i^2"] += myX.getX() * otherX.getX();
expression["ij"] += myX.getX() * otherY.getX();
expression["ik"] += myX.getX() * otherZ.getX();
// third group
expression["j"] += myY.getX() * otherW.getX();
expression["ji"] += myY.getX() * otherX.getX();
expression["j^2"] += myY.getX() * otherY.getX();
expression["jk"] += myY.getX() * otherZ.getX();
// fourth group
expression["k"] += myZ.getX() * otherW.getX();
expression["ki"] += myZ.getX() * otherX.getX();
expression["kj"] += myZ.getX() * otherY.getX();
expression["k^2"] += myZ.getX() * otherZ.getX();
// convert ij and ji
expression["k"] += expression["ij"];
expression["k"] += -expression["ji"];
// convert jk and kj
expression["i"] += expression["jk"];
expression["i"] += -expression["kj"];
// convert ki and ik
expression["j"] += expression["ki"];
expression["j"] += -expression["ik"];
// then convert any squared i|j|k to -1 and store result in "w" index
double iSquaredResult = expression["i^2"] * -1;
double jSquaredResult = expression["j^2"] * -1;
double kSquaredResult = expression["k^2"] * -1;
expression["w"] += iSquaredResult;
expression["w"] += jSquaredResult;
expression["w"] += kSquaredResult;
result.setW(expression["w"]);
result.setX(expression["i"]);
result.setY(expression["j"]);
result.setZ(expression["k"]);
if (debugMode)
{
Console.WriteLine("\n\nexpression results:");
foreach (var key in expression.Keys)
{
Console.WriteLine("{0} - {1}", key, expression[key]);
}
}
return result;
}
public Quaternion inverse()
{
double s = this.getW();
Dictionary<string, double> v = new Dictionary<string, double>();
Quaternion result = new Quaternion(0, 0, 0, 0);
v.Add("x", this.getX());
v.Add("y", this.getY());
v.Add("z", this.getZ());
double totalToDivideBy = Math.Pow(s, 2)
+ Math.Pow(v["x"], 2)
+ Math.Pow(v["y"], 2)
+ Math.Pow(v["z"], 2);
result.setW(s / totalToDivideBy);
result.setX(-v["x"] / totalToDivideBy);
result.setY(-v["y"] / totalToDivideBy);
result.setZ(-v["z"] / totalToDivideBy);
// final round for output (if set)
if (roundingForOutput)
{
result.setW(Math.Round(result.getW(), 4));
result.setX(Math.Round(result.getX(), 4));
result.setY(Math.Round(result.getY(), 4));
result.setZ(Math.Round(result.getZ(), 4));
}
return result;
}
public Quaternion divide(Quaternion other)
{
// multiply q by inverse of other
Quaternion result = new Quaternion(0, 0, 0, 0);
Quaternion inverseOther = other.inverse();
result = this.multiply(inverseOther);
return result;
}
public Quaternion divide(Quaternion other)
{
// multiply q by inverse of other
Quaternion result = new Quaternion(0, 0, 0, 0);
Quaternion inverseOther = other.inverse();
result = this.multiply(inverseOther);
// final round for output (if set)
if (roundingForOutput)
{
result.setW(Math.Round(result.getW(), 4));
result.setX(Math.Round(result.getX(), 4));
result.setY(Math.Round(result.getY(), 4));
result.setZ(Math.Round(result.getZ(), 4));
}
return result;
}
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");
}
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";
}
private Quaternion parseStringToQuat(String input)
{
String[] inputString = input.Split(',');
Quaternion result = new Quaternion(Convert.ToDouble(inputString[0]), Convert.ToDouble(inputString[1]), Convert.ToDouble(inputString[2]), Convert.ToDouble(inputString[3]));
return result;
}
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;
}
}