Angle between two 3D vectors

Question

I have a series of vertices (pink) that I want to rotate so that one edge of the pattern of vertices matches with the edge of the triangle (white).

To do this, I first create two vectors to represent the edges: floretAB and triangleAB (green). I then find the cross product of the two to get an axis around which I can rotate the vertices (red).

I then get the angle between the two vectors, and use that, with the rotation axis to create a quaternion. Finally, I rotate all the vertices around the quaternion.

Before rotation

_

What rotation should produce

_

However, although the vertices correctly rotate around the quaternion, the angle is not coming out correct, as illustrated here:

This is the code I'm using to get the angle between the two vectors. I don't understand what I'm doing wrong:

double[] cross = new double[3];
crossProduct(floretAB.mX, floretAB.mY, floretAB.mZ, triangleAB.mX, triangleAB.mY, triangleAB.mZ, cross);
double dot = dotProduct(floretAB.mX, floretAB.mY, floretAB.mZ, triangleAB.mX, triangleAB.mY, triangleAB.mZ);
double crossMag = Math.sqrt(cross[0]*cross[0] + cross[1]*cross[1] + cross[2]*cross[2]);
double angle = Math.atan2(crossMag, dot);

public static double dotProduct(double vector1X,double vector1Y,double vector1Z,double vector2X,double vector2Y,double vector2Z){

    return vector1X*vector2X + vector1Y*vector2Y + vector1Z*vector2Z;

}

public static void crossProduct(double vector1X,double vector1Y,double vector1Z,double vector2X,double vector2Y,double vector2Z, double[] outputArray){

    outputArray[0] = vector1Y*vector2Z - vector1Z*vector2Y;     
    outputArray[1] = vector1Z*vector2X - vector1X*vector2Z;
    outputArray[2] = vector1X*vector2Y - vector1Y*vector2X;

}

Any help with this would be most appreciated as it is really bugging me.

Thanks, James

Edit: Here is the rest of the code:

        // get floret p1,p2 vector
    // get triangle p1,p2 vector
    Vector3D floretAB = new Vector3D(florets3D[0], florets3D[7]);
    // get triangle p1,p2 vector
    Vector3D triangleAB = new Vector3D(triangle[0], triangle[1]);

    // get rotation axis (cross) and angle (dot)

    /*
    double[] cross = new double[3];
    crossProduct(floretAB.mX, floretAB.mY, floretAB.mZ, triangleAB.mX, triangleAB.mY, triangleAB.mZ, cross);
    double dotMag = floretAB.getMagnitude() * triangleAB.getMagnitude();
    double dot = dotProduct(floretAB.mX, floretAB.mY, floretAB.mZ, triangleAB.mX, triangleAB.mY, triangleAB.mZ) / dotMag;
    double angle = Math.acos(dot);
    */

    double[] cross = new double[3];
    crossProduct(floretAB.mX, floretAB.mY, floretAB.mZ, triangleAB.mX, triangleAB.mY, triangleAB.mZ, cross);
    double dot = dotProduct(floretAB.mX, floretAB.mY, floretAB.mZ, triangleAB.mX, triangleAB.mY, triangleAB.mZ);
    double crossMag = Math.sqrt(cross[0]*cross[0] + cross[1]*cross[1] + cross[2]*cross[2]);
    double angle = Math.atan2(crossMag, dot);

    // rotate floret so p1,p2 vector matches with triangle p1,p2 vector     
    double[] newVerts = new double[3];
    Quaternion quat = new Quaternion(cross[0], cross[1], cross[2], angle);
    for(int i = 0;i<numfloretVerts;i++){
        Vertex3D vert = florets3D[i];
        quat.RotateVector(vert.getmX(), vert.getmY(), vert.getmZ(), newVerts);
        vert.setmX(newVerts[0]);
        vert.setmY(newVerts[1]);
        vert.setmZ(newVerts[2]);
    }

_

public class Vector3D {

public double mX;
public double mY;
public double mZ;

public Vertex3D point;

/**
 * Constructs a vector from two points. The new vector is normalised
 * 
 * @param point1
 * @param point2
 */
public Vector3D(Vertex3D point1, Vertex3D point2){
    mX = point2.getmX() - point1.getmX();
    mY = point2.getmY() - point1.getmY();
    mZ = point2.getmZ() - point1.getmZ();
    normalise();
    point = point1;
}

/**
 * Normalises the vector
 */
public void normalise(){
    double magnitude = Math.sqrt(mX*mX + mY*mY + mZ*mZ);
    if(magnitude!=0){
        mX /= magnitude;
        mY /= magnitude;
        mZ /= magnitude;
    }
}

/**
 * 
 * @return the magnitude of the vector
 */
public double getMagnitude(){
    return Math.sqrt(mX*mX + mY*mY + mZ*mZ);
}

}

_

public class Quaternion {

private static final double TOLERANCE = 0.00001f;

double w;
double x;
double y;
double z;

public Quaternion(double axisX, double axisY, double axisZ, double angleInRadians){
    setAxisAngle(axisX, axisY, axisZ, angleInRadians);      
}

public void Normalise(){

    // Don't normalize if we don't have to
    double mag2 = w * w + x * x + y * y + z * z;
    if (Math.abs(mag2) > TOLERANCE && Math.abs(mag2 - 1.0f) > TOLERANCE) {
        double mag = (double) Math.sqrt(mag2);
        w /= mag;
        x /= mag;
        y /= mag;
        z /= mag;
    }

}

public void getConjugate(double[] outputArray){

    outputArray[0] = w;
    outputArray[1] = -x;
    outputArray[2] = -y;
    outputArray[3] = -z;

}

public void Multiply(double[] aq, double[] rq, double[] outputArray){

    outputArray[0] = aq[0] * rq[0] - aq[1] * rq[1] - aq[2] * rq[2] - aq[3] * rq[3];
    outputArray[1] = aq[0] * rq[1] + aq[1] * rq[0] + aq[2] * rq[3] - aq[3] * rq[2];
    outputArray[2] = aq[0] * rq[2] + aq[2] * rq[0] + aq[3] * rq[1] - aq[1] * rq[3];
    outputArray[3] = aq[0] * rq[3] + aq[3] * rq[0] + aq[1] * rq[2] - aq[2] * rq[1];

}

private double[] vecQuat = new double[4];
private double[] resQuat = new double[4];
private double[] thisQuat = new double[4];

private double[] conj = new double[4];

/**
 * Rotates a vector (or point) around this axis-angle
 * 
 * @param vectorX the x component of the vector (or point)
 * @param vectorY the y component of the vector (or point)
 * @param vectorZ the z component of the vector (or point)
 * @param outputArray the array in which the results will be stored
 */
public void RotateVector(double vectorX, double vectorY, double vectorZ, double[] outputArray){

    vecQuat[0] = 0.0f;
    vecQuat[1] = vectorX;
    vecQuat[2] = vectorY;
    vecQuat[3] = vectorZ;

    thisQuat[0] = w;
    thisQuat[1] = x;
    thisQuat[2] = y;
    thisQuat[3] = z;

    getConjugate(conj);
    Multiply(vecQuat,conj,resQuat);
    Multiply(thisQuat,resQuat,vecQuat);

    outputArray[0] = vecQuat[1];
    outputArray[1] = vecQuat[2];
    outputArray[2] = vecQuat[3];

}

/**
 * set Quaternion by providing axis-angle form
 */
public void setAxisAngle(double axisX, double axisY, double axisZ, double angleInRadians){
    w = (double) Math.cos( angleInRadians/2);
    x = (double) (axisX * Math.sin( angleInRadians/2 ));
    y = (double) (axisY * Math.sin( angleInRadians/2 ));
    z = (double) (axisZ * Math.sin( angleInRadians/2 ));

    Normalise();
}
}

Answer 1

I think you've overcomplicated your maths.

Given two unit vectors (you did say they were normalised) then the magnitude of the cross product is equal to sin(theta). There shouldn't be any need to invoke the dot product or atan2.

You possibly also need to normalise the cross product vector result too before you create the quaternion - it depends on your implementation of new Quaternion(x, y, z, theta) and whether it requires [x, y, z] to be normalised or not.

Answer 2

I think the problem is that you evaluate the angle in the wrong way. If I understand correctly what are you trying to achieve, then you need the angle between 2 green lines. You correctly evaluate the dot product between 2 green lines using the definition:

(a, b) = a1*b1 + a2*b2 + a3*b3.

But dot product can also be evaluated like this:

(a, b) = |a|*|b|*cos(theta)

So you can evaluate cos(theta) - the cosine of angle between 2 green lines - like this:

cos(theta) = (a1*b1 + a2*b2 + a3*b3) / (|a|*|b|)

But I would use yet another approach. I would normalize both vectors at first (i.e. converted them to unit-vectors). You can do this by dividing each vector's component by the vector's length (sqrt(x1*x1 + y1*y1 + z1*z1)) Then you will have the following:

(aa, bb) = cos(theta)

where aa is normalized a and bb is normalized b.

I hope this helps.

Answer 3

The stated answers are correct for real numbers but can loose accuracy near certain angles when computed with floating point numbers. For arcos() when the angle is near zero or PI, and for arcsin() near pi/2 and –pi/2, as many a half the significant figures can be lost. A method that is more robust and only suffers a few rounding errors uniformly over the whole range from and including zero to and including PI, assuming the input vectors are unit length is:

public double AngleBetween(Vector3D a, Vector3D b)
{
    return 2.0d * Math.atan((a-b).Length/(a+b).Length);
}

Note, this gives the unoriented angle between the two vectors. A reference for this and attributed to Kahan may be found at: http://www.cs.berkeley.edu/~wkahan/MathH110/Cross.pdf