// ---------------------------- VSALGEB.H -----------------------
// VSpace 3d library.
// Designed and written by Javier Ar‚valo Baeza.
// Types and functions for linear algebra.

#ifndef _VSALGEB_H_
#define _VSALGEB_H_

#ifndef _VSBASE_H_
#include "vsbase.h"
#endif

#include <math.h>

#ifdef __cplusplus
extern "C" {
#endif

/*
 * World coordinate system is as follows:
 *  RIGHT HANDED.
 *  Positive rotation (angle > 0) around an axis is such that viewed from
 *      the positive part of the axis towards the origin, the rotation is
 *      COUNTERCLOCKWISE.
 *
 * The transformation applied to an object are like this:
 *  The object is at position P, rotated with angles Ax, Ay, Az
 *  The transformed object is, for each vertex V:
 *      V' = RyúRxúRzúV + P
 *  This convention means that the principal (symmetry) axis of a flight
 *  object (e.g. a plane) is its Z axis, the tilting (up-down) axis is
 *  the X axis, and finally the horizontal direction is around the Y
 *  axis.
 *
 * Note that the camera coordinate system has positive Z values INTO the
 * screen and positive Y values DOWNWARDS. This is to be taken care of
 * when creating the camera (inverse) rotation matrix and during projection.

 * I think of vectors as column vectors, therefore the composition is
 *
 *  V' = RùV
 *
 *  ³ X'³   Ú   ¿  ³ X ³
 *  ³ Y'³ = ³ R ³ù ³ Y ³
 *  ³ Z'³   À   Ù  ³ Z ³

 * So to compose a rotation to another, like
 *
 *  V" = AùV' = Aù(RùV) = (AùR)ùV = MùV
 * Here, A is pre-composed to R to form M, or R is post-composed to A.
 *

 * Rotation around X axis:
 *      1       0       0
 *      0   cos à  -sin à
 *      0   sin à   cos à

 * Rotation around Y axis:
 *  cos à       0   sin à
 *      0       1       0
 * -sin à       0   cos à

 * Rotation around Z axis:
 *  cos à  -sin à       0
 *  sin à   cos à       0
 *      0       0       1

 * REMEMBER these are applied in this order: Z, X, Y.

 * Note also  that matrix[1][2] refers to row 1, column 2 or
    .  .  .
    .  .  X
    .  .  .

 */

// -------------------------------------------

#ifndef PI
#define PI (3.141592648)
#endif

#ifndef TWOPI
#define TWOPI (2.0*PI)
#endif

#ifndef HALFPI
#define HALFPI (PI/2.0)
#endif

typedef float VSALG_TAngle, *VSALG_PAngle;

typedef float VSALG_TCoord, *VSALG_PCoord;

typedef union {
    VSALG_TCoord m[2];
    struct {
        VSALG_TCoord x, y;
    };
} VSALG_T2DPoint, *VSALG_P2DPoint;

typedef struct {
    VSALG_T2DPoint v0, v1;
} VSALG_T2DBox, *VSALG_P2DBox;

typedef union {
    VSALG_TCoord m[3];
    struct {
        VSALG_TCoord x, y, z;
    };
} VSALG_T3DPoint, *VSALG_P3DPoint;

typedef struct {
    VSALG_T3DPoint v0, v1;
} VSALG_T3DBox, *VSALG_P3DBox;

typedef union {
    VSALG_TAngle m[3];
    struct {
        VSALG_TAngle x, y, z;
    };
} VSALG_T3DAngles, *VSALG_P3DAngles;

typedef union {
    VSALG_TCoord m[4];
    struct {
        VSALG_TCoord s;
        VSALG_T3DPoint v;
    };
} VSALG_TQuaternion, *VSALG_PQuaternion;

typedef union {
    VSALG_TCoord m[3][3];
    struct {
        VSALG_T3DPoint v[3];
    };
} VSALG_T3DMatrix, *VSALG_P3DMatrix;

typedef union {
    VSALG_TCoord m[4];
    struct {
        VSALG_T3DPoint p;
        VSALG_TCoord d;
    };
} VSALG_T3DPlane, *VSALG_P3DPlane;

// -------------------------------------------
// Useful constants.

    // [0,0,0].
extern const VSALG_T3DPoint VSALG_ZeroVector;

    /* [1,0,0
        0,1,0
        0,0,1] */
extern const VSALG_T3DMatrix VSALG_UnitMatrix;

// ---------------------------------------------------------

    // ------ #defines for raw speed
    // - Remember: #DEFINES
    // This means you shouldn't pass complex parameters such as p->v[i*k]
    // and NEVER pass expressions that modify a value, like i++ or (a=b)
    // Generally, pre-assign pointers, and also use pointers for traversing
    // an array (let's face it, the usual thing you will do).

    // Nearest integer towards 0
#define VSALG_FloorType(a,type) ((a) > 0 ? ((type))(a) : -((type))(a))
#define VSALG_Floor(a) VSALG_FloorType((a),int)

    // Nearest integer away from 0
#define VSALG_CeilingType(a,type) ((a) > 0 ? 1 + ((type))(a) : -(1 + ((type))(-a)))
#define VSALG_Ceiling(a) VSALG_CeilingType((a),int)

    // Nearest integer
#define VSALG_RoundType(a,type) ((a) > 0 ? ((type))(a+0.5) : -(((type))(0.5-a)))
#define VSALG_Round(a) VSALG_RoundType((a),int)

    // Linear interpolation from l (a == 0) to h (a == b)
#define VSALG_Lerp(l,h,a,b) ((l) + (((h) - (l)) * (a) / (b)))
    // In case you know the h and l come inverted compared to a previous
    // Lerp, and want _exactly_ the same result (typical).
#define VSALG_InvLerp(l,h,a,b) VSALG_Lerp((h),(l),(a),(b))

    // Limit value between low & high limits
#define VSALG_Clamp(v,l,h) ((v) < (l) ? (l) : (v) > (h) ? (h) : (l))

    // Power of 2
#define VSALG_Pow2(a) ((a)*(a))

    // ---------------------------------
    // Structure operations.

    // Copy a 2D vector.
#define VSALG_Copy2DVector(a,b) (       \
    (a)->x=(b)->x,                    \
    (a)->y=(b)->y)

    // Copy a 3D vector.
#define VSALG_Copy3DVector(a,b) (           \
    (a)->m[0]=(b)->m[0],                    \
    (a)->m[1]=(b)->m[1],                    \
    (a)->m[2]=(b)->m[2])

    // Copy a 3D matrix.
#define VSALG_Copy3DMatrix(a,b) (RepMovsd((a)->m,(b)->m,sizeof(VSALG_T3DMatrix)/4))

#define VSALG_CopyQuat(a,b) (VSALG_Copy3DVector(&(a)->v, &(b)->v), (a)->s = (b)->s)

    // Assign a 2D vector.
#define VSALG_Set2D(p,_x,_y) ((p)->x=(_x),(p)->y=(_y))

    // Assign a 3D vector.
#define VSALG_Set3D(p,_x,_y,_z) ((p)->x=(_x),(p)->y=(_y),(p)->z=(_z))

    // Negate a 2D vector.
#define VSALG_Neg2D(p,v) ((p)->x=-(v)->x,(p)->y=-(v)->y)

    // Negate a 3D vector.
#define VSALG_Neg3D(p,v) ((p)->x=-(v)->x,(p)->y=-(v)->y,(p)->z=-(v)->z)

    // Add a vector 'vec' to a position 'pos' vector into 'dest'.
#define VSALG_Add2D(d,p,v) (     \
    ((d)->x = (p)->x + (v)->x),  \
    ((d)->y = (p)->y + (v)->y))

    // Add a vector 'vec' to a position 'pos' vector into 'dest'.
#define VSALG_Add3D(d,p,v) (     \
    ((d)->x = (p)->x + (v)->x),  \
    ((d)->y = (p)->y + (v)->y),  \
    ((d)->z = (p)->z + (v)->z))

    // Subtract a vector 'vec' from a position 'pos' vector into 'dest'.
#define VSALG_Sub2D(d,p,v) (     \
    ((d)->x = (p)->x - (v)->x),  \
    ((d)->y = (p)->y - (v)->y))

    // Subtract a vector 'vec' from a position 'pos' vector into 'dest'.
#define VSALG_Sub3D(d,p,v) (     \
    ((d)->x = (p)->x - (v)->x),  \
    ((d)->y = (p)->y - (v)->y),  \
    ((d)->z = (p)->z - (v)->z))

    // Scale a vector
#define VSALG_Scale2D(d,p,a) (  \
    ((d)->x = (p)->x*(a)),    \
    ((d)->y = (p)->y*(a)))

#define VSALG_Scale3D(d,p,a) (  \
    ((d)->x = (p)->x*(a)),    \
    ((d)->y = (p)->y*(a)),    \
    ((d)->z = (p)->z*(a)))

    // Dot product of two vectors.
#define VSALG_Dot2D(v1,v2) (     \
    (v1)->x * (v2)->x +        \
    (v1)->y * (v2)->y)

#define VSALG_Dot3D(v1,v2) (     \
    (v1)->x * (v2)->x +        \
    (v1)->y * (v2)->y +        \
    (v1)->z * (v2)->z)

    // Linear interpolations
#define VSALG_Lerp2D(p,l,h,a,b) (               \
    ((p)->x = VSALG_Lerp((l)->x,(h)->x,(a),(b))), \
    ((p)->y = VSALG_Lerp((l)->y,(h)->y,(a),(b))))

#define VSALG_InvLerp2D(p,l,h,a,b) (               \
    ((p)->x = VSALG_InvLerp((l)->x,(h)->x,(a),(b))), \
    ((p)->y = VSALG_InvLerp((l)->y,(h)->y,(a),(b))))

#define VSALG_Lerp3D(p,l,h,a,b) (               \
    ((p)->x = VSALG_Lerp((l)->x,(h)->x,(a),(b))), \
    ((p)->y = VSALG_Lerp((l)->y,(h)->y,(a),(b))), \
    ((p)->z = VSALG_Lerp((l)->z,(h)->z,(a),(b))))

#define VSALG_InvLerp3D(p,l,h,a,b) (               \
    ((p)->x = VSALG_InvLerp((l)->x,(h)->x,(a),(b))), \
    ((p)->y = VSALG_InvLerp((l)->y,(h)->y,(a),(b))), \
    ((p)->z = VSALG_InvLerp((l)->z,(h)->z,(a),(b))))

    // Linear combinations
#define VSALG_Combine2D(p,a,b,as,bs) (      \
    ((p)->x = (as)*(a)->x + (bs)*(b)->x), \
    ((p)->y = (as)*(a)->y + (bs)*(b)->y))

#define VSALG_Combine3D(p,a,b,as,bs) (      \
    ((p)->x = (as)*(a)->x + (bs)*(b)->x), \
    ((p)->y = (as)*(a)->y + (bs)*(b)->y), \
    ((p)->z = (as)*(a)->z + (bs)*(b)->z))

    // Multiply vectors
#define VSALG_Mul2D(p,a,b) (    \
    ((p)->x = (a)->x*(b)->x), \
    ((p)->y = (a)->y*(b)->y))

#define VSALG_Mul3D(p,a,b) (    \
    ((p)->x = (a)->x*(b)->x), \
    ((p)->y = (a)->y*(b)->y), \
    ((p)->z = (a)->z*(b)->z))

    // Dot product of a vector an another vector from a to b.
#define VSALG_Dot2DSub(v,a,b) (       \
    ((v)->x * ((b)->x - (a)->x)) +  \
    ((v)->y * ((b)->y - (a)->y)))

#define VSALG_Dot3DSub(v,a,b) (       \
    ((v)->x * ((b)->x - (a)->x)) +  \
    ((v)->y * ((b)->y - (a)->y)) +  \
    ((v)->z * ((b)->z - (a)->z)))

    // ------------------------

    // Scale a matrix.
#define VSALG_ScaleMatrix(d,m,a) (     \
    (VSALG_Scale3D((d),(m)->v[0],(a)), \
    (VSALG_Scale3D((d),(m)->v[1],(a)), \
    (VSALG_Scale3D((d),(m)->v[2],(a)))
/*
    // Transform a position 'pos' by a rotation matrix 'm' into 'dest'.
    // NOTE: 'dest' *must* be different from 'pos'.
#define VSALG_Rotate(d,p,m) (                                               \
    ((d)->x=(r)->m[0][0]*(p)->x+(r)->m[0][1]*(p)->y+(r)->m[0][2]*(p)->z), \
    ((d)->y=(r)->m[1][0]*(p)->x+(r)->m[1][1]*(p)->y+(r)->m[1][2]*(p)->z), \
    ((d)->z=(r)->m[2][0]*(p)->x+(r)->m[2][1]*(p)->y+(r)->m[2][2]*(p)->z))

    // Transform a position 'pos' by a rotation matrix 'm' into 'dest'.
    // And add a translation.
    // NOTE: 'dest' *must* be different from 'pos'.
#define VSALG_RotateAdd(d,p,m,t) (                                      \
    ((d)->x=(r)->m[0][0]*(p)->x+(r)->m[0][1]*(p)->y+(r)->m[0][2]*(p)->z+(t)->x), \
    ((d)->y=(r)->m[1][0]*(p)->x+(r)->m[1][1]*(p)->y+(r)->m[1][2]*(p)->z+(t)->y), \
    ((d)->z=(r)->m[2][0]*(p)->x+(r)->m[2][1]*(p)->y+(r)->m[2][2]*(p)->z+(t)->z))

    // Transform a position 'pos' by a rotation matrix 'm' into 'dest'.
    // And sub a translation.
    // NOTE: 'dest' *must* be different from 'pos'.
#define VSALG_RotateSub(d,p,m,t) (                                      \
    ((d)->x=(r)->m[0][0]*(p)->x+(r)->m[0][1]*(p)->y+(r)->m[0][2]*(p)->z-(t)->x), \
    ((d)->y=(r)->m[1][0]*(p)->x+(r)->m[1][1]*(p)->y+(r)->m[1][2]*(p)->z-(t)->y), \
    ((d)->z=(r)->m[2][0]*(p)->x+(r)->m[2][1]*(p)->y+(r)->m[2][2]*(p)->z-(t)->z))
*/
    // Transform a position 'pos' by a rotation matrix 'm' into 'dest'.
    // NOTE: 'dest' *must* be different from 'pos'.
#define VSALG_Rotate(d,p,r) (                \
    ((d)->x = VSALG_Dot3D(&(r)->v[0], (p))), \
    ((d)->y = VSALG_Dot3D(&(r)->v[1], (p))), \
    ((d)->z = VSALG_Dot3D(&(r)->v[2], (p))))

    // Transform a position 'pos' by a rotation matrix 'm' into 'dest'.
    // And add a translation.
    // NOTE: 'dest' *must* be different from 'pos'.
#define VSALG_RotateAdd(d,p,t,r) (                   \
    ((d)->x = VSALG_Dot3D(&(r)->v[0], (p)) + (t)->x), \
    ((d)->y = VSALG_Dot3D(&(r)->v[1], (p)) + (t)->y), \
    ((d)->z = VSALG_Dot3D(&(r)->v[2], (p)) + (t)->z))

    // Transform a position 'pos' by a rotation matrix 'm' into 'dest'.
    // And sub a translation.
    // NOTE: 'dest' *must* be different from 'pos'.
#define VSALG_RotateSub(d,p,t,r) (                   \
    ((d)->x = VSALG_Dot3D(&(r)->v[0], (p)) - (t)->x), \
    ((d)->y = VSALG_Dot3D(&(r)->v[1], (p)) - (t)->y), \
    ((d)->z = VSALG_Dot3D(&(r)->v[2], (p)) - (t)->z))

    // Project a coordinate with a focus
#define VSALG_ProjectCoord(c,z,f) ((c)*(f)/(z))

    // Project a 3D position 'v' into a 2D position 'p' with a focus
#define VSALG_Project(p,v,fx,fy) (                   \
    (p)->x = VSALG_ProjectCoord((v)->x,(v)->z,(fx)), \
    (p)->y = VSALG_ProjectCoord((v)->y,(v)->z,(fy)))

    // Project a 3D position 'v' into a 2D position 'p' with a focus and
    // a safe clip plane.
#define VSALG_ProjectSafe(p,v,fx,fy,c) (  \
    ((v)->z > (c)) ? (                  \
        VSALG_Project((p),(v),(fx),(fy))  \
     ) : (                              \
        (p)->x = VSALG_ProjectCoord((v)->x,(c),(fx)), \
        (p)->y = VSALG_ProjectCoord((v)->y,(c),(fy)) \
    ))

    // ------------------------

    // Distance from point to (normalized) plane.
#define VSALG_DistancePlane(p,v) (VSALG_Dot3D(&(p)->p, (v)) + (p)->d)

// ----------------------------------------------------

    // ------ Bulky functions.
    // Note that all these support that dest be one of the parameters also,
    // with a little loss in efficiency.

extern VSALG_TCoord VSALG_Length2D(VSALG_P2DPoint v1);

extern VSALG_TCoord VSALG_Length3D(VSALG_P3DPoint v1);

    // Make a vector of length 1. Returns dest.
extern VSALG_P2DPoint VSALG_Normalize2D(VSALG_P2DPoint dest, VSALG_P2DPoint v1);

extern VSALG_P3DPoint VSALG_Normalize3D(VSALG_P3DPoint dest, VSALG_P3DPoint v1);

    // Distance between points
extern VSALG_TCoord VSALG_Distance2D(VSALG_P2DPoint v0, VSALG_P2DPoint v1);

extern VSALG_TCoord VSALG_Distance3D(VSALG_P3DPoint v0, VSALG_P3DPoint v1);

    // Cross product. Returns dest.
extern VSALG_P3DPoint VSALG_Cross3D(VSALG_P3DPoint dest, VSALG_P3DPoint v1, VSALG_P3DPoint v2);

    // Matrix operations.

    // Transpose matrix. Remember, for orthogonal matrices, this is also
    // the inverse.
extern VSALG_P3DMatrix VSALG_Transpose(VSALG_P3DMatrix dest, VSALG_P3DMatrix m1);

    // Compose a rotation matrix 'm1' by the left with 'm2' into 'dest'.
    // Returns dest. This is a plain matrix multiplication.
extern VSALG_P3DMatrix VSALG_Compose(VSALG_P3DMatrix dest, VSALG_P3DMatrix m1, VSALG_P3DMatrix m2);

    // Compose the inverse of a rotation matrix 'm1' by the left with 'm2' into
    // 'dest'. Returns dest. This is a plain matrix multiplication.
extern VSALG_P3DMatrix VSALG_PreComposeInv(VSALG_P3DMatrix dest, VSALG_P3DMatrix m1, VSALG_P3DMatrix m2);

    // Compose a rotation matrix 'm1' by the left with the inverse of 'm2' into
    // 'dest'. Returns dest. This is a plain matrix multiplication.
extern VSALG_P3DMatrix VSALG_PostComposeInv(VSALG_P3DMatrix dest, VSALG_P3DMatrix m1, VSALG_P3DMatrix m2);

    // Build a rotation matrix around the X axis.
extern VSALG_P3DMatrix VSALG_CreateXRotation(VSALG_P3DMatrix m, VSALG_TAngle angle);

    // Build a rotation matrix around the Y axis.
extern VSALG_P3DMatrix VSALG_CreateYRotation(VSALG_P3DMatrix m, VSALG_TAngle angle);

    // Build a rotation matrix around the Z axis.
extern VSALG_P3DMatrix VSALG_CreateZRotation(VSALG_P3DMatrix m, VSALG_TAngle angle);

    // Build a rotation matrix around an arbitrary axis defined by a unit vector.
extern VSALG_P3DMatrix VSALG_CreateAxisRotation(VSALG_P3DMatrix m, VSALG_P3DPoint v, VSALG_TAngle angle);

    // Compose a rotation matrix around the X axis by the left into m.
extern VSALG_P3DMatrix VSALG_PreComposeXRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle);

    // Compose a rotation matrix around the Y axis by the left into m.
extern VSALG_P3DMatrix VSALG_PreComposeYRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle);

    // Compose a rotation matrix around the Z axis by the left into m.
extern VSALG_P3DMatrix VSALG_PreComposeZRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle);

    // Post Compose a rotation matrix around the X axis by the right into m.
extern VSALG_P3DMatrix VSALG_PostComposeXRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle);

    // Post Compose a rotation matrix around the Y axis by the right into m.
extern VSALG_P3DMatrix VSALG_PostComposeYRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle);

    // Post Compose a rotation matrix around the Z axis by the right into m.
extern VSALG_P3DMatrix VSALG_PostComposeZRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_TAngle angle);

    // Compose a rotation matrix around the axis by the left into m.
extern VSALG_P3DMatrix VSALG_PreComposeAxisRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_P3DPoint v, VSALG_TAngle angle);

    // Post Compose a rotation matrix around the axis by the left into m.
extern VSALG_P3DMatrix VSALG_PostComposeAxisRotation(VSALG_P3DMatrix dest, VSALG_P3DMatrix m, VSALG_P3DPoint v, VSALG_TAngle angle);

    // Advance a vector by some other vector rotated by a matrix.
    // dest = v + d*M
extern VSALG_P3DPoint VSALG_AdvanceRotated(VSALG_P3DPoint dest, VSALG_P3DPoint v, VSALG_P3DPoint d, VSALG_P3DMatrix m);

    // Create a matrix that looks from a point to another.
extern VSALG_P3DMatrix VSALG_LookAt(VSALG_P3DMatrix m, VSALG_P3DPoint from, VSALG_P3DPoint to);

    // --------------------------

    // Gram-Schmidt orthonormalization of a 3D matrix.
extern VSALG_P3DMatrix VSALG_Ortho3DMatrix(VSALG_P3DMatrix dest, VSALG_P3DMatrix m);

    // Arvo's (Gems1) method for finding a transformed bounding box.
    // Box is (min,max), not (center,radius). v' = Mv + t
extern VSALG_P3DBox VSALG_RotateAddBounds(VSALG_P3DBox dest, VSALG_P3DBox box, VSALG_P3DMatrix m, VSALG_P3DPoint t);

    // --------------------------
    // Quaternion stuff (maybe we'll find a use for it).
    // Mostly based on code by Gavin Bell & Doug Moore.

    // Generate a quaternion for a given axis & angle around it.
extern VSALG_PQuaternion VSALG_Axis2Quat(VSALG_PQuaternion dest, VSALG_TAngle s, VSALG_P3DPoint v);

    // Normalize a quaternion.
extern VSALG_PQuaternion VSALG_NormalizeQuat(VSALG_PQuaternion dest, VSALG_PQuaternion q);

    // Add a quaternion to another.
extern VSALG_PQuaternion VSALG_AddQuat(VSALG_PQuaternion dest, VSALG_PQuaternion q0, VSALG_PQuaternion q1);

    // Build a rotation matrix for a unit quaternion.
extern VSALG_P3DMatrix VSALG_Quat2Matrix(VSALG_P3DMatrix dest, VSALG_PQuaternion q);

    // Spherical interpolation of unit quaternions. 0 <= t <= 1
extern VSALG_PQuaternion VSALG_QuatSlerp(VSALG_PQuaternion dest,
                                         VSALG_PQuaternion q1,
                                         VSALG_PQuaternion q2,
                                         VSALG_TCoord t);


#ifdef __cplusplus
}
#endif

#endif

// ---------------------------- VSALGEB.H -----------------------
