/**************************************************************************
 *{@C
 *      Copyright:      1988-2023 Paul Obermeier (obermeier@poSoft.de)
 *
 *      Module:         Utilities
 *      Filename:       UT_VecAngle.c
 *
 *      Author:         Paul Obermeier
 *
 *      Description:    Functions to determine angles between vectors.
 *
 *      Additional documentation:
 *                      None.
 *
 *      Exported functions:
 *                      UT_VecAngle2D
 *                      UT_VecAngle3D
 *                      UT_VecSignAngle2D
 *                      UT_VecSignAngle3D
 *
 **************************************************************************/

#include <stdio.h>
#include <math.h>

#include "UT_Compat.h"
#include "UT_Const.h"
#include "UT_Macros.h"

#include "UT_Vector.h"

/***************************************************************************
 *[@e
 *      Name:           UT_VecAngle2D
 *
 *      Usage:          Find the angle between two vectors.
 *
 *      Synopsis:       Float32 UT_VecAngle2D
 *                              (const UT_Vec2D v1, const UT_Vec2D v2)
 *
 *      Description:    UT_VecAngle2D returns the (counterclockwise) angle
 *                      between v1 and v2. V1 and v2 must have length 1.0.
 *
 *                      There is an identical function "UT_DVecAngle2D" for
 *                      dealing with vectors in double precision. 
 *                      
 *      Return value:   A number between 0.0 and (2.0 * UT_PI).
 *
 *      See also:
 *
 ***************************************************************************/

Float32 UT_VecAngle2D (const UT_Vec2D v1, const UT_Vec2D v2)
{
    Float32 v1_x_v2, v1_dot_v2, angle;

    v1_x_v2 = UT_VecCrossProd2D (v1, v2);
    v1_dot_v2 = UT_VecDotProd2D (v1, v2);
    if (UT_ABS (v1_dot_v2) < 0.7071067811) {
        angle = acos (v1_dot_v2);
    } else {
        angle = asin (UT_ABS (v1_x_v2));
        if (v1_dot_v2 < 0.0) {
            angle = UT_PI - angle;
        }
    }
    if (v1_x_v2 < 0.0) {
        angle = 2.0 * UT_PI - angle;
    }
    return angle;
}

Float64 UT_DVecAngle2D (const UT_DVec2D v1, const UT_DVec2D v2)
{
    Float64 v1_x_v2, v1_dot_v2, angle;

    v1_x_v2 = UT_DVecCrossProd2D (v1, v2);
    v1_dot_v2 = UT_DVecDotProd2D (v1, v2);
    if (UT_ABS (v1_dot_v2) < 0.7071067811) {
        angle = acos (v1_dot_v2);
    } else {
        angle = asin (UT_ABS (v1_x_v2));
        if (v1_dot_v2 < 0.0) {
            angle = UT_PI - angle;
        }
    }
    if (v1_x_v2 < 0.0) {
        angle = 2.0 * UT_PI - angle;
    }
    return angle;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecAngle3D
 *
 *      Usage:          Find the angle between two vectors.
 *
 *      Synopsis:       Float32 UT_VecAngle3D
 *                              (const UT_Vec3D v1, const UT_Vec3D v2,
 *                              const UT_Vec3D nrm)
 *                      
 *      Description:    UT_VecAngle3D returns the angle between v1 and v2.
 *                      The angle is measured counterclockwise, looking
 *                      at v1 and v2 in the opposite direction of nrm.
 *                      V1, v2 and nrm must have length 1.0.
 *                      
 *                      There is an identical function "UT_DVecAngle3D" for
 *                      dealing with vectors in double precision. 
 *
 *      Return value:   A number between 0.0 and (2.0 * UT_PI).
 *
 *      See also:
 *
 ***************************************************************************/

Float32 UT_VecAngle3D (const UT_Vec3D v1, const UT_Vec3D v2, const UT_Vec3D nrm)
{
    UT_Vec3D v1_x_v2;
    Float32 v1_dot_v2, angle;

    UT_VecCrossProd3D (v1, v2, v1_x_v2);
    v1_dot_v2 = UT_VecDotProd3D (v1, v2);
    if (UT_ABS (v1_dot_v2) < 0.5 * sqrt (2.0)) { /* OPA Was 0.7071067811 */
        angle = acos (v1_dot_v2);
    } else {
        angle = asin (UT_VecLength3D (v1_x_v2));
        if (v1_dot_v2 < 0.0) {
            angle = UT_PI - angle;
        }
    }
    if (UT_VecDotProd3D (v1_x_v2, nrm) < 0.0) {
        angle = 2.0 * UT_PI - angle;
    }
    return angle;
}

Float64 UT_DVecAngle3D (const UT_DVec3D v1, const UT_DVec3D v2, const UT_DVec3D nrm)
{
    UT_DVec3D v1_x_v2;
    Float64 v1_dot_v2, angle;

    UT_DVecCrossProd3D (v1, v2, v1_x_v2);
    v1_dot_v2 = UT_DVecDotProd3D (v1, v2);
    if (UT_ABS (v1_dot_v2) < 0.5 * sqrt (2.0)) { /* OPA: Was 0.7071067811 */
        angle = acos (v1_dot_v2);
    } else {
        angle = asin (UT_DVecLength3D (v1_x_v2));
        if (v1_dot_v2 < 0.0) {
            angle = UT_PI - angle;
        }
    }
    if (UT_DVecDotProd3D (v1_x_v2, nrm) < 0.0) {
        angle = 2.0 * UT_PI - angle;
    }
    return angle;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecSignAngle2D
 *
 *      Usage:          Compare the angle between two vectors to UT_PI.
 *
 *      Synopsis:       Int32 UT_VecSignAngle2D
 *                              (const UT_Vec2D v1, const UT_Vec2D v2)
 *
 *      Description:    UT_VecSignAngle2D finds out if the angle as returned
 *                      by UT_VecAngle2D is less than or greater than UT_PI.
 *
 *                      Note: UT_VecSignAngle2D does not actually compute the
 *                      angle; thus it is faster than UT_VecAngle2D.
 *                      V1, v2 and nrm do not have to have length 1.0.

 *                      There is an identical function "UT_DVecSignAngle2D" for
 *                      dealing with vectors in double precision. 
 *
 *      Return value:   1 if the angle is less than UT_PI, 0 if the angle
 *                      is 0.0 or UT_PI, -1 if the angle is greater than
 *                      UT_PI (or less than 0.0).
 *
 *      See also:
 *
 ***************************************************************************/

Int32 UT_VecSignAngle2D (const UT_Vec2D v1, const UT_Vec2D v2)
{
    Float32 v1_x_v2;

    v1_x_v2 = UT_VecCrossProd2D (v1, v2);
    return UT_SGN (v1_x_v2);
}

Int32 UT_DVecSignAngle2D (const UT_DVec2D v1, const UT_DVec2D v2)
{
    Float64 v1_x_v2;

    v1_x_v2 = UT_DVecCrossProd2D (v1, v2);
    return UT_SGN (v1_x_v2);
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecSignAngle3D
 *
 *      Usage:          Compare the angle between two vectors to UT_PI.
 *
 *      Synopsis:       Int32 UT_VecSignAngle3D
 *                              (const UT_Vec3D v1, const UT_Vec3D v2,
 *                              const UT_Vec3D nrm)
 *
 *      Description:    UT_VecSignAngle3D finds out if the angle as returned
 *                      by UT_VecAngle3D is less than or greater than UT_PI.
 *
 *                      Note: UT_VecSignAngle3D does not actually compute the
 *                      angle; thus it is faster than UT_VecAngle3D.
 *                      V1, v2 and nrm do not have to have length 1.0.
 *
 *                      There is an identical function "UT_DVecSignAngle3D" for
 *                      dealing with vectors in double precision. 
 *
 *      Return value:   1 if the angle is less than UT_PI, 0 if the angle
 *                      is 0.0 or UT_PI, -1 if the angle is greater than
 *                      UT_PI (or less than 0.0).
 *
 *      See also:
 *
 ***************************************************************************/

Int32 UT_VecSignAngle3D (const UT_Vec3D v1, const UT_Vec3D v2, const UT_Vec3D nrm)
{
    UT_Vec3D v1_x_v2;
    Float32 x_dot_nrm;

    UT_VecCrossProd3D (v1, v2, v1_x_v2);
    x_dot_nrm = UT_VecDotProd3D (v1_x_v2, nrm);
    return UT_SGN (x_dot_nrm);
}

Int32 UT_DVecSignAngle3D (const UT_DVec3D v1, const UT_DVec3D v2, const UT_DVec3D nrm)
{
    UT_DVec3D v1_x_v2;
    Float64 x_dot_nrm;

    UT_DVecCrossProd3D (v1, v2, v1_x_v2);
    x_dot_nrm = UT_DVecDotProd3D (v1_x_v2, nrm);
    return UT_SGN (x_dot_nrm);
}
