/* revision 6 */

/* @2010 Kamil Kaminski
 *
 * math3d.c
 * 
 * this code is not yet endian aware, and it won't be, screw powerpc
 * the style of the syntax is original k&r except there's \n
 * after the opening { and extra space after if statement,
 * and for/while loops
 */

#include "math3d.h"

void m3dFindNormalf(M3DVector3f result, const M3DVector3f point1, 
                   const M3DVector3f point2, const M3DVector3f point3)
{
    M3DVector3f v1, v2;

    /* calculate two vectors from the three points, assumes 
     * counter clockwise winding
     */
    v1[0] = point1[0] - point2[0];
    v1[1] = point1[1] - point2[1];
    v1[2] = point1[2] - point2[2];

    v2[0] = point2[0] - point3[0];
    v2[1] = point2[1] - point3[1];
    v2[2] = point2[2] - point3[2];

    /* take the cross product of the two vectors to get the normal vector */
    m3dCrossProductf(result, v1, v2);
}


void m3dLoadIdentity33f(M3DMatrix33f m)
{
    /* don't be fooled, this is still column major */
    static M3DMatrix33f identity = { 1.0f, 0.0f, 0.0f,
                                     0.0f, 1.0f, 0.0f,
                                     0.0f, 0.0f, 1.0f };

    memcpy(m, identity, sizeof(M3DMatrix33f));
}


void m3dLoadIdentity44f(M3DMatrix44f m) /* 4x4 float */
{
    /* don't be fooled, this is still column major */
    static M3DMatrix44f identity = { 1.0f, 0.0f, 0.0f, 0.0f,
                                     0.0f, 1.0f, 0.0f, 0.0f,
                                     0.0f, 0.0f, 1.0f, 0.0f,
                                     0.0f, 0.0f, 0.0f, 1.0f };

    memcpy(m, identity, sizeof(M3DMatrix44f));
}

/* calculate the plane equation of the plane that the three specified points lay in
 * the points are given in clockwise winding order, with normal pointing out of clockwise face
 * planeEq contains the a, b, c, and d of the plane equation coefficients
 */ 
void m3dGetPlaneEquationf(M3DVector4f planeEq, const M3DVector3f p1,
                         const M3DVector3f p2, const M3DVector3f p3)
{
    /* get two vectors... do the cross product */
    M3DVector3f v1, v2;

    /* v1 = p3 - p1 */
    v1[0] = p3[0] - p1[0];
    v1[1] = p3[1] - p1[1];
    v1[2] = p3[2] - p1[2];

    /* v2 = p2 - p1 */
    v2[0] = p2[0] - p1[0];
    v2[1] = p2[1] - p1[1];
    v2[2] = p2[2] - p1[2];

    /* unit normal to plane - Not sure which is the best way here */
    m3dCrossProductf(planeEq, v1, v2);
    m3dNormalizeVectorf(planeEq);
    /* back substitute to get d */
    planeEq[3] = -(planeEq[0] * p3[0] + planeEq[1] * p3[1] + planeEq[2] * p3[2]);
}

/* creates a 4x4 rotation matrix, takes radians not degrees */
void m3dRotationMatrix44f(M3DMatrix44f m, float angle, float x, float y, float z)
{
    float mag, s, c;
    float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;

    s = (float) (sin(angle));
    c = (float) (cos(angle));

    mag = (float) (sqrt(x*x + y*y + z*z));

    /* identity matrix */
    if (mag == 0.0f)
    {
        m3dLoadIdentity44f(m);
        return;
    }

    /* rotation matrix is normalized */
    x /= mag;
    y /= mag;
    z /= mag;

    #define M(row,col) m[col*4+row]

    xx = x * x;
    yy = y * y;
    zz = z * z;
    xy = x * y;
    yz = y * z;
    zx = z * x;

    xs = x * s;
    ys = y * s;
    zs = z * s;
    one_c = 1.0f - c;

    M(0,0) = (one_c * xx) + c;
    M(0,1) = (one_c * xy) - zs;
    M(0,2) = (one_c * zx) + ys;
    M(0,3) = 0.0f;

    M(1,0) = (one_c * xy) + zs;
    M(1,1) = (one_c * yy) + c;
    M(1,2) = (one_c * yz) - xs;
    M(1,3) = 0.0f;

    M(2,0) = (one_c * zx) - ys;
    M(2,1) = (one_c * yz) + xs;
    M(2,2) = (one_c * zz) + c;
    M(2,3) = 0.0f;

    M(3,0) = 0.0f;
    M(3,1) = 0.0f;
    M(3,2) = 0.0f;
    M(3,3) = 1.0f;

    #undef M
}

/*  create a projection to "squish" an object into the plane.
 * use m3dGetPlaneEquationf(planeEq, point1, point2, point3); to get a plane equation
 */
void m3dMakePlanarShadowMatrixf(M3DMatrix44f proj, const M3DVector4f planeEq,
                                const M3DVector3f vLightPos)
{
    /* these just make the code below easier to read */
    float a = planeEq[0];
    float b = planeEq[1];
    float c = planeEq[2];
    float d = planeEq[3];

    float dx = -vLightPos[0];
    float dy = -vLightPos[1];
    float dz = -vLightPos[2];

    /* now build the projection matrix */
    proj[0] = b * dy + c * dz;
    proj[1] = -a * dy;
    proj[2] = -a * dz;
    proj[3] = 0.0;

    proj[4] = -b * dx;
    proj[5] = a * dx + c * dz;
    proj[6] = -b * dz;
    proj[7] = 0.0;

    proj[8] = -c * dx;
    proj[9] = -c * dy;
    proj[10] = a * dx + b * dy;
    proj[11] = 0.0;

    proj[12] = -d * dx;
    proj[13] = -d * dy;
    proj[14] = -d * dz;
    proj[15] = a * dx + b * dy + c * dz;
    /* shadow matrix ready */
}

#define A(row,col)  a[(col<<2)+row]
#define B(row,col)  b[(col<<2)+row]
#define P(row,col)  product[(col<<2)+row]

void m3dMatrixMultiply44f(M3DMatrix44f product, const M3DMatrix44f a, 
                         const M3DMatrix44f b)
{
    int i;
    for (i = 0; i < 4; i++)
    {
        float ai0 = A(i, 0), ai1 = A(i, 1), ai2 = A(i, 2), ai3 = A(i, 3);
        P(i, 0) = ai0 * B(0, 0) + ai1 * B(1, 0) + ai2 * B(2, 0) + ai3 * B(3, 0);
        P(i, 1) = ai0 * B(0, 1) + ai1 * B(1, 1) + ai2 * B(2, 1) + ai3 * B(3, 1);
        P(i, 2) = ai0 * B(0, 2) + ai1 * B(1, 2) + ai2 * B(2, 2) + ai3 * B(3, 2);
        P(i, 3) = ai0 * B(0, 3) + ai1 * B(1, 3) + ai2 * B(2, 3) + ai3 * B(3, 3);
    }
}

#undef A
#undef B
#undef P