We would probably want to see the source code for
Quaternion.mul(Quaternion,Quaternion).
Additional info which may help:
* Did you write the Quaternion class, or did a 3rd party write it?
* If you know, are instances of the Quaternion immutable?
- Oliver
Oliver,
Here's the class in its entirety...still under development:
package angle;
/**
* @author mfeher
*
*/
public class Quaternion
{
// Methods
// Constructors
Quaternion() {
q0 = 0.0;
q1 = 0.0;
q2 = 0.0;
q3 = 0.0;
}
Quaternion(double q, double qi, double qj, double qk) {
q0 = q;
q1 = qi;
q2 = qj;
q3 = qk;
}
// Conversion methods
// Method to convert a quaternion to Euler angles
static EulerAngles quat2euler(Quaternion q) {
// First we need to convert to a DCM representation
Matrix3x3D DCM = new Matrix3x3D();
DCM = Quaternion.quat2dcm(q);
// Now, using the DCM, extract the Euler angles in radians
double rollRad = Math.atan(DCM.item(1, 2) / DCM.item(2, 2));
double pitchRad = Math.asin(-1*DCM.item(0,2));
double yawRad = Math.atan(DCM.item(0, 1) / DCM.item(0, 0));
// Convert the Euler angles in radians to degrees and store
them
Angle roll = new Angle(Angle.rad2deg(rollRad));
Angle pitch = new Angle(Angle.rad2deg(pitchRad));
Angle yaw = new Angle(Angle.rad2deg(yawRad));
// Set the resultant Euler angles
EulerAngles euler = new EulerAngles(roll, pitch, yaw);
return euler;
}
// Defined mathematical operations
// All of these operations are static so they can be used without
having
// *this* quaternion possess real values
static Quaternion add(Quaternion qa, Quaternion qb) {
// This method performs the defined operation of addition on
two quaternions
// i.e. qc = qa + qb
Quaternion qc = new Quaternion();
qc.q0 = (qa.q0 + qb.q0);
qc.q1 = (qa.q1 + qb.q1);
qc.q2 = (qa.q2 + qb.q2);
qc.q3 = (qa.q3 + qb.q3);
return qc;
}
static Quaternion neg(Quaternion q) {
// This method performs the defined operation of negation of a
quaternion
// i.e. q_neg = neg(q)
Quaternion q_neg = new Quaternion();
q_neg.q0 = -1*q.q0;
q_neg.q1 = -1*q.q1;
q_neg.q2 = -1*q.q2;
q_neg.q3 = -1*q.q3;
return q_neg;
}
static Quaternion sub(Quaternion qa, Quaternion qb) {
// This method performs the defined operation of subtraction
on two quaternions
// i.e. qc = (qa - qb)
Quaternion qc = new Quaternion();
qc.q0 = (qa.q0 - qb.q0);
qc.q1 = (qa.q1 - qb.q1);
qc.q2 = (qa.q2 - qb.q2);
qc.q3 = (qa.q3 - qb.q3);
return qc;
}
static Quaternion mul(Quaternion qa, Quaternion qb) {
// This method performs the defined operation of
multiplication on two quaternions
// i.e. qc = (qa * qb)
// Later on we may implement 3x3 matrix operations whereby we
can
// use those appropriately rather than doing it "directly"
like we do
// here (the less elegant but no less accurate approach).
Quaternion qc = new Quaternion();
qc.q0 = (qa.q0*qb.q0 - qa.q1*qb.q1 - qa.q2*qb.q2 -
qa.q3*qb.q3);
qc.q1 = (qa.q0*qb.q1 + qa.q1*qb.q0 + qa.q2*qb.q3 -
qa.q3*qb.q2);
qc.q2 = (qa.q0*qb.q2 + qa.q2*qb.q0 + qa.q3*qb.q1 -
qa.q1*qb.q3);
qc.q3 = (qa.q0*qb.q3 + qa.q3*qb.q0 + qa.q1*qb.q2 -
qa.q2*qb.q1);
// Note that we can use this method to multiply vector
quaternions, which
// simply means that the scalar parts of such vectors are zero
(i.e. we
// don't need a separate multiplication method to achieve
this).
return qc;
}
static Quaternion inv(Quaternion q) {
// This method returns the inverse of a given quaternion
// i.e. q_inv = q^-1
Quaternion q_inv = new Quaternion();
return q_inv;
}
static Quaternion div(Quaternion qa, Quaternion qb) {
// This method performs the defined operation of division on
two quaternions
// i.e. q_div = qa/qb
Quaternion q_div = new Quaternion();
// First, take the inverse of qb, the denominator
Quaternion qb_inv = Quaternion.inv(qb);
// Now multiply it by the numerator
q_div = Quaternion.mul(qa, qb_inv);
return q_div;
}
static Quaternion conj(Quaternion q) {
// This method returns the conjugate of a given quaternion
// i.e. q_conj = ~q
Quaternion q_conj = new Quaternion();
return q_conj;
}
static Quaternion unit(Quaternion q) {
// This method returns the unit quaternion of a given
quaternion
// i.e. q_unit = |q|
Quaternion q_unit = new Quaternion();
return q_unit;
}
static double index(Quaternion p, Quaternion q) {
// Method to calculate the index of quaternions. If the
quaternions are
// right quaternions, then the index of a right quaternion is
the axis of
// the quaternion scaled by the ratio of lengths.
return 0.0;
}
static double scalar(Quaternion q) {
// Method to determine the scalar part of a quaternion
// Part of the representation in rectangular (Cartesian)
coordinate
return q0;
}
static Vector right(Quaternion q) {
// Method to determine the right part of a quaternion
// Part of the representation in rectangular (Cartesian)
coordinate
Vector right = new Vector(q1, q2, q3);
return right;
}
static void tensor(Quaternion q) {
// Method to determine the tensor part of a quaternion
// Part of the representation in polar coordinates
}
static void versor(Quaternion q) {
// Method to determine the versor part of a quaternion
// Part of the representation in polar coordinates
}
// Later on we may add the "scalar" and "vector" getters for a
quaternion.
// TODO: Change to regular DCM when we implement that
static Matrix3x3D quat2dcm(Quaternion q) {
// Method to convert a given quaternion into a DCM
Matrix3x3D DCM = new Matrix3x3D(); // Change to regular DCM
when we implement that
DCM.array[0][0] = ((q.q0 * q.q0) + (q.q1 * q.q1) - (q.q2 *
q.q2) - (q.q3 * q.q3));
DCM.array[0][1] = 2 * ((q.q1 * q.q2) + (q.q0 * q.q3));
DCM.array[0][2] = 2 * ((q.q1 * q.q3) - (q.q0 * q.q2));
DCM.array[1][0] = 2 * ((q.q1 * q.q2) - (q.q0 * q.q3));
DCM.array[1][1] = ((q.q0 * q.q0) - (q.q1 * q.q1) + (q.q2 *
q.q2) - (q.q3 * q.q3));
DCM.array[1][2] = 2 * ((q.q2 * q.q3) + (q.q0 * q.q1));
DCM.array[2][0] = 2 * ((q.q1 * q.q3) + (q.q0 * q.q2));
DCM.array[2][1] = 2 * ((q.q2 * q.q3) - (q.q0 * q.q1));
DCM.array[2][2] = ((q.q0 * q.q0) - (q.q1 * q.q1) - (q.q2 *
q.q2) + (q.q3 * q.q3));
return DCM;
}
// Lastly, we implement the all-important rotate method - but we
will need
// a 3D vector class to do this
Vector rot(Quaternion q, Vector v_old) {
// Rotate a vector using defined quaternion transformation
Vector v_new = new Vector();
// Perform transformation operations
// v_new = q * v_old * q_inv
return v_new;
}
// Getter methods
double q0() { return q0; }
double q1() { return q1; }
double q2() { return q2; }
double q3() { return q3; }
// Print/inspect methods
// May want to implement toString here instead
void print() {
// Method to print out this quaternion.
// We need some simple handling based on the sign of each
component of
// the quaternion.
String s0, s1, s2, s3;
s0 = String.format("%.3f", q0);
if (q1 < 0)
{
s1 = String.format("- %.3f", -1*q1);
}
else
{
s1 = String.format("+ %.3f", q1);
}
if (q2 < 0)
{
s2 = String.format("- %.3f", -1*q2);
}
else
{
s2 = String.format("+ %.3f", q2);
}
if (q3 < 0)
{
s3 = String.format("- %.3f", -1*q3);
}
else
{
s3 = String.format("+ %.3f", q3);
}
String q = String.format("quaternion: %s %si %sj %sk", s0, s1,
s2, s3);
System.out.println(q);
}
// Attributes
private static double q0;
private static double q1;
private static double q2;
private static double q3;
}
