source: GTP/trunk/App/Demos/Vis/CHC_revisited/MathStuff.c @ 2642

//Copyright and Disclaimer:
//This code is copyright Vienna University of Technology, 2004.

#include <memory.h>
#include <math.h>
#include <float.h>
#include <stdlib.h>
#include <malloc.h>
#include <stdio.h>
#include "MathStuff.h"

#ifndef M_PI
#define M_PI (double)3.14159265358979323846
#endif

const double DOUBLE_MAX = DBL_MAX;
const double PI = M_PI;
const double PI_2 = M_PI/2;
const double PI_180 = M_PI/180;
const Vector3 ZERO = {0.0, 0.0, 0.0};
const Vector3 UNIT_X = {1.0, 0.0, 0.0};
const Vector3 UNIT_Y = {0.0, 1.0, 0.0};
const Vector3 UNIT_Z = {0.0, 0.0, 1.0};

const Matrix4x4 IDENTITY = {1.0, 0.0, 0.0, 0.0, 
                                                        0.0, 1.0, 0.0, 0.0,
                                                        0.0, 0.0, 1.0, 0.0,
                                                        0.0, 0.0, 0.0, 1.0};

double maximum(const double a, const double b) {
        return ( (a > b)? (a):(b) );

}

void clamp(double* value, const double min, const double max) {
        if( (*value) > max) {
                (*value) = max;
        }
        else {
                if( (*value) < min) {
                        (*value) = min;
                }
        }
}

double absDouble(const double a) {
        return ( (a < 0.0)? (-a):(a) );
}

double signDouble(const double a) {
        return ( (a < 0.0)? (-1.0f): ( (a == 0.0)? (0.0f):(1.0f) ) );
}

double coTan(const double vIn) {
        return (double)-tan(vIn+PI_2);
}

double relativeEpsilon(const double a, const double epsilon) {
        double relEpsilon = maximum(absDouble(a*epsilon),epsilon);
        return relEpsilon;
}

int alike(const double a, const double b, const double epsilon) {
        if(a == b) {
                return 1;
        }
        {
                double relEps = relativeEpsilon(a,epsilon);
                return (a-relEps <= b) && (b <= a+relEps);
        }
}



int alikeVector3(const Vector3 a, const Vector3 b, const double epsilon) {
        return 
                alike(a[0],b[0],epsilon) &&
                alike(a[1],b[1],epsilon) &&
                alike(a[2],b[2],epsilon);
}

void linCombVector3(Vector3 result, const Vector3 pos, const Vector3 dir, const double t) {
        int i;
        for(i = 0; i < 3; i++) {
                result[i] = pos[i]+t*dir[i];
        }
}

double squaredLength(const Vector3 vec) {
        int i;
        double tmp = 0.0;
        for(i = 0; i < 3; i++) {
                tmp += vec[i]*vec[i];
        }
        return tmp;
}

void normalize(Vector3 vec) {
        int i;
        const double len = (double)(1.0/sqrt(squaredLength(vec)));
        for(i = 0; i < 3; i++) {
                vec[i] *= len;
        }
}

void copyNormalize(Vector3 vec, const Vector3 input) {
        copyVector3(vec,input),
        normalize(vec);
}

/*      | a1 a2 |
        | b1 b2 | calculate the determinent of a 2x2 matrix*/
double det2x2(const double a1, const double a2,
                        const double b1, const double b2) {
        return a1*b2 - b1*a2;
}

/*      | a1 a2 a3 |
        | b1 b2 b3 |
        | c1 c2 c3 | calculate the determinent of a 3x3 matrix*/
double det3x3(const double a1, const double a2, const double a3,
                          const double b1, const double b2, const double b3,
                          const double c1, const double c2, const double c3) {
        return a1*det2x2(b2,b3,c2,c3) - b1*det2x2(a2,a3,c2,c3) +
                        c1*det2x2(a2,a3,b2,b3);
}
void cross(Vector3 result, const Vector3 a, const Vector3 b) {
        result[0] =  det2x2(a[1],b[1],a[2],b[2]);
        result[1] = -det2x2(a[0],b[0],a[2],b[2]);
        result[2] =  det2x2(a[0],b[0],a[1],b[1]);
}

double dot(const Vector3 a, const Vector3 b) {
        return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}

void look(Matrix4x4 output, const Vector3 pos, const Vector3 dir, const Vector3 up) {
        Vector3 dirN;
        Vector3 upN;
        Vector3 lftN;

        cross(lftN,dir,up);
        normalize(lftN);

        cross(upN,lftN,dir);
        normalize(upN);
        copyNormalize(dirN,dir);

        output[ 0] = lftN[0];
        output[ 1] = upN[0];
        output[ 2] = -dirN[0];
        output[ 3] = 0.0;

        output[ 4] = lftN[1];
        output[ 5] = upN[1];
        output[ 6] = -dirN[1];
        output[ 7] = 0.0;

        output[ 8] = lftN[2];
        output[ 9] = upN[2];
        output[10] = -dirN[2];
        output[11] = 0.0;

        output[12] = -dot(lftN,pos);
        output[13] = -dot(upN,pos);
        output[14] = dot(dirN,pos);
        output[15] = 1.0;
}

void makeScaleMtx(Matrix4x4 output, const double x, const double y, const double z) {
        output[ 0] = x;
        output[ 1] = 0.0;
        output[ 2] = 0.0;
        output[ 3] = 0.0;

        output[ 4] = 0.0;
        output[ 5] = y;
        output[ 6] = 0.0;
        output[ 7] = 0.0;

        output[ 8] = 0.0;
        output[ 9] = 0.0;
        output[10] = z;
        output[11] = 0.0;

        output[12] = 0.0;
        output[13] = 0.0;
        output[14] = 0.0;
        output[15] = 1.0;
}

void makeTranslationMtx(Matrix4x4 output, Vector3 translation) {
        output[ 0] = 1.0;
        output[ 1] = 0.0;
        output[ 2] = 0.0;
        output[ 3] = 0;//translation[0];

        output[ 4] = 0.0;
        output[ 5] = 1.0;
        output[ 6] = 0.0;
        output[ 7] = 0;//translation[1];

        output[ 8] = 0.0;
        output[ 9] = 0.0;
        output[10] = 1.0;
        output[11] = 0;//translation[2];

        output[12] = translation[0];//0.0;
        output[13] = translation[1];//0.0;
        output[14] = translation[2];//0.0;
        output[15] = 1.0;
}

void makeRotationZMtx(Matrix4x4 output, const double rad)
{
        output[ 0] = cos(rad);
        output[ 1] = sin(rad);
        output[ 2] = 0.0;
        output[ 3] = 0.0;

        output[ 4] = -sin(rad);
        output[ 5] = cos(rad);
        output[ 6] = 0.0;
        output[ 7] = 0.0;

        output[ 8] = 0.0;
        output[ 9] = 0.0;
        output[10] = 1.0;
        output[11] = 0.0;

        output[12] = 0.0;
        output[13] = 0.0;
        output[14] = 0.0;
        output[15] = 1.0;
}

void makeRotationXMtx(Matrix4x4 output, const double rad)
{
        output[ 0] = 1.0;
        output[ 1] = 0.0;
        output[ 2] = 0.0;
        output[ 3] = 0.0;

        output[ 4] = 0.0;
        output[ 5] = cos(rad);
        output[ 6] = sin(rad);
        output[ 7] = 0.0;

        output[ 8] = 0.0;
        output[ 9] = -sin(rad);
        output[10] = cos(rad);
        output[11] = 0.0;

        output[12] = 0.0;
        output[13] = 0.0;
        output[14] = 0.0;
        output[15] = 1.0;
}

void makeRotationYMtx(Matrix4x4 output, const double rad)
{
        output[ 0] = cos(rad);
        output[ 1] = 0.0;
        output[ 2] = -sin(rad);
        output[ 3] = 0.0;

        output[ 4] = 0.0;
        output[ 5] = 1.0;
        output[ 6] = 0.0;
        output[ 7] = 0.0;

        output[ 8] = sin(rad);
        output[ 9] = 0.0;
        output[10] = cos(rad);
        output[11] = 0.0;

        output[12] = 0.0;
        output[13] = 0.0;
        output[14] = 0.0;
        output[15] = 1.0;
}

void scaleTranslateToFit(Matrix4x4 output, const Vector3 vMin, const Vector3 vMax) {
        output[ 0] = 2/(vMax[0]-vMin[0]);
        output[ 4] = 0;
        output[ 8] = 0;
        output[12] = -(vMax[0]+vMin[0])/(vMax[0]-vMin[0]);

        output[ 1] = 0;
        output[ 5] = 2/(vMax[1]-vMin[1]);
        output[ 9] = 0;
        output[13] = -(vMax[1]+vMin[1])/(vMax[1]-vMin[1]);

        output[ 2] = 0;
        output[ 6] = 0;
        output[10] = 2/(vMax[2]-vMin[2]);
        output[14] = -(vMax[2]+vMin[2])/(vMax[2]-vMin[2]);

        output[ 3] = 0;
        output[ 7] = 0;
        output[11] = 0;
        output[15] = 1;
}

void perspectiveRad(Matrix4x4 output, const double vFovy, const double vAspect,
                                                  const double vNearDis, const double vFarDis) {
        const double f = coTan(vFovy/2);
        const double dif = 1/(vNearDis-vFarDis);

        output[ 0] = f/vAspect;
        output[ 4] = 0;
        output[ 8] = 0;
        output[12] = 0;

        output[ 1] = 0;
        output[ 5] = f;
        output[ 9] = 0;
        output[13] = 0;

        output[ 2] = 0;
        output[ 6] = 0;
        output[10] = (vFarDis+vNearDis)*dif;
        output[14] = 2*vFarDis*vNearDis*dif;

        output[ 3] = 0;
        output[ 7] = 0;
        output[11] = -1;
        output[15] = 0;
}

void perspectiveDeg(Matrix4x4 output, const double vFovy, const double vAspect,
                                        const double vNearDis, const double vFarDis) {
        perspectiveRad(output,vFovy*PI_180,vAspect,vNearDis,vFarDis);
}

void invert(Matrix4x4 output, const Matrix4x4 i) {
        double a11 =  det3x3(i[5],i[6],i[7],i[9],i[10],i[11],i[13],i[14],i[15]);
        double a21 = -det3x3(i[1],i[2],i[3],i[9],i[10],i[11],i[13],i[14],i[15]);
        double a31 =  det3x3(i[1],i[2],i[3],i[5],i[6],i[7],i[13],i[14],i[15]);
        double a41 = -det3x3(i[1],i[2],i[3],i[5],i[6],i[7],i[9],i[10],i[11]);

        double a12 = -det3x3(i[4],i[6],i[7],i[8],i[10],i[11],i[12],i[14],i[15]);
        double a22 =  det3x3(i[0],i[2],i[3],i[8],i[10],i[11],i[12],i[14],i[15]);
        double a32 = -det3x3(i[0],i[2],i[3],i[4],i[6],i[7],i[12],i[14],i[15]);
        double a42 =  det3x3(i[0],i[2],i[3],i[4],i[6],i[7],i[8],i[10],i[11]);

        double a13 =  det3x3(i[4],i[5],i[7],i[8],i[9],i[11],i[12],i[13],i[15]);
        double a23 = -det3x3(i[0],i[1],i[3],i[8],i[9],i[11],i[12],i[13],i[15]);
        double a33 =  det3x3(i[0],i[1],i[3],i[4],i[5],i[7],i[12],i[13],i[15]);
        double a43 = -det3x3(i[0],i[1],i[3],i[4],i[5],i[7],i[8],i[9],i[11]);

        double a14 = -det3x3(i[4],i[5],i[6],i[8],i[9],i[10],i[12],i[13],i[14]);
        double a24 =  det3x3(i[0],i[1],i[2],i[8],i[9],i[10],i[12],i[13],i[14]);
        double a34 = -det3x3(i[0],i[1],i[2],i[4],i[5],i[6],i[12],i[13],i[14]);
        double a44 =  det3x3(i[0],i[1],i[2],i[4],i[5],i[6],i[8],i[9],i[10]);

        double det = (i[0]*a11) + (i[4]*a21) + (i[8]*a31) + (i[12]*a41);
        double oodet = 1/det;

        output[ 0] = a11*oodet;
        output[ 1] = a21*oodet;
        output[ 2] = a31*oodet;
        output[ 3] = a41*oodet;

        output[ 4] = a12*oodet;
        output[ 5] = a22*oodet;
        output[ 6] = a32*oodet;
        output[ 7] = a42*oodet;

        output[ 8] = a13*oodet;
        output[ 9] = a23*oodet;
        output[10] = a33*oodet;
        output[11] = a43*oodet;

        output[12] = a14*oodet;
        output[13] = a24*oodet;
        output[14] = a34*oodet;
        output[15] = a44*oodet;
}

void multUnSave(Matrix4x4 output, const Matrix4x4 a, const Matrix4x4 b) {
        const int SIZE = 4;
        int iCol;
        for(iCol = 0; iCol < SIZE; iCol++) {
                const int cID = iCol*SIZE;
                int iRow;
                for(iRow = 0; iRow < SIZE; iRow++) {
                        const int id = iRow+cID;
                        int k;
                        output[id] = a[iRow]*b[cID];
                        for(k = 1; k < SIZE; k++) {
                                output[id] += a[iRow+k*SIZE]*b[k+cID];
                        }
                }
        }
}

void mult(Matrix4x4 output, const Matrix4x4 a, const Matrix4x4 b) {
        if(a == output) {
                Matrix4x4 tmpA;
                copyMatrix(tmpA,a);
                if(b == output) {
                        multUnSave(output,tmpA,tmpA);
                }
                else {
                        multUnSave(output,tmpA,b);
                }
        }
        else {
                if(b == output) {
                        Matrix4x4 tmpB;
                        copyMatrix(tmpB,b);
                        multUnSave(output,a,tmpB);
                }
                else {
                        multUnSave(output,a,b);
                }
        }
}

void mulHomogenPoint(Vector3 output, const Matrix4x4 m, const Vector3 v) {
        //if v == output -> overwriting problems -> so store in temp
        double x = m[0]*v[0] + m[4]*v[1] + m[ 8]*v[2] + m[12];
        double y = m[1]*v[0] + m[5]*v[1] + m[ 9]*v[2] + m[13];
        double z = m[2]*v[0] + m[6]*v[1] + m[10]*v[2] + m[14];
        double w = m[3]*v[0] + m[7]*v[1] + m[11]*v[2] + m[15];

        output[0] = x/w;
        output[1] = y/w;
        output[2] = z/w;
}

void appendToCubicHull(Vector3 min, Vector3 max, const Vector3 v) {
        int j;
        for(j = 0; j < 3; j++) {
                if(v[j] < min[j]) {
                        min[j] = v[j];
                }
                else if(v[j] > max[j]) {
                        max[j] = v[j];
                }
        }
}

void calcCubicHull(Vector3 min, Vector3 max, const Vector3* ps, const int size) {
        if(size > 0) {
                int i;
                copyVector3(min,ps[0]);
                copyVector3(max,ps[0]);
                for(i = 1; i < size; i++) {
                        appendToCubicHull(min,max,ps[i]);
                }
        }
}

void calcViewFrustumWorldCoord(Vector3x8 points, const Matrix4x4 invEyeProjView) {
        const struct AABox box = {
                {-1, -1, -1},
                {1, 1, 1}
        };
        int i;
        calcAABoxPoints(points,box); //calc unit cube corner points
        for(i = 0; i < 8; i++) {
                mulHomogenPoint(points[i],invEyeProjView,points[i]); //camera to world frame
        }
        //viewFrustumWorldCoord[0] == near-bottom-left
        //viewFrustumWorldCoord[1] == near-bottom-right
        //viewFrustumWorldCoord[2] == near-top-right
        //viewFrustumWorldCoord[3] == near-top-left
        //viewFrustumWorldCoord[4] == far-bottom-left
        //viewFrustumWorldCoord[5] == far-bottom-right
        //viewFrustumWorldCoord[6] == far-top-right
        //viewFrustumWorldCoord[7] == far-top-left
}

void calcViewFrustObject(struct Object* obj, const Vector3x8 p) {
        int i;
        Vector3* ps;
        objectSetSize(obj,6);
        for(i = 0; i < 6; i++) {
                vecPointSetSize(&obj->poly[i],4);
        }
        //near poly ccw
        ps = obj->poly[0].points;
        for(i = 0; i < 4; i++) {
                copyVector3(ps[i],p[i]);
        }
        //far poly ccw
        ps = obj->poly[1].points;
        for(i = 4; i < 8; i++) {
                copyVector3(ps[i-4],p[11-i]);
        }
        //left poly ccw
        ps = obj->poly[2].points;
        copyVector3(ps[0],p[0]);
        copyVector3(ps[1],p[3]);
        copyVector3(ps[2],p[7]);
        copyVector3(ps[3],p[4]);
        //right poly ccw
        ps = obj->poly[3].points;
        copyVector3(ps[0],p[1]);
        copyVector3(ps[1],p[5]);
        copyVector3(ps[2],p[6]);
        copyVector3(ps[3],p[2]);
        //bottom poly ccw
        ps = obj->poly[4].points;
        copyVector3(ps[0],p[4]);
        copyVector3(ps[1],p[5]);
        copyVector3(ps[2],p[1]);
        copyVector3(ps[3],p[0]);
        //top poly ccw
        ps = obj->poly[5].points;
        copyVector3(ps[0],p[6]);
        copyVector3(ps[1],p[7]);
        copyVector3(ps[2],p[3]);
        copyVector3(ps[3],p[2]);
}

void transformVecPoint(struct VecPoint* poly, const Matrix4x4 xForm) {
        if(0 != poly) {
                int i;
                for(i = 0; i < poly->size; i++) {
                        mulHomogenPoint(poly->points[i],xForm,poly->points[i]);
                }
        }
}

void transformObject(struct Object* obj, const Matrix4x4 xForm) {
        if(0 != obj) {
                int i;
                for(i = 0; i < obj->size; i++) {
                        transformVecPoint( &(obj->poly[i]) ,xForm);
                }
        }
}

void calcObjectCubicHull(Vector3 min, Vector3 max, const struct Object obj) {
        if(0 < obj.size) {
                int i;
                calcCubicHull(min,max,obj.poly[0].points,obj.poly[0].size);
                for(i = 1; i < obj.size; i++) {
                        struct VecPoint* p = &(obj.poly[i]);
                        int j;
                        for(j = 0; j < p->size; j++) {
                                appendToCubicHull(min,max,p->points[j]);
                        }
                }
        }
}

int findSamePointInVecPoint(const struct VecPoint poly, const Vector3 p) {
        int i;
        for(i = 0; i < poly.size; i++) {
                if(alikeVector3(poly.points[i],p,(double)0.001)) {
                        return i;
                }
        }
        return -1;
}

int findSamePointInObjectAndSwapWithLast(struct Object *inter, const Vector3 p) {
        int i;
        if(0 == inter) {
                return -1;
        }
        if(1 > inter->size) {
                return -1;
        }
        for(i = inter->size; i > 0; i--) {
                struct VecPoint* poly = &(inter->poly[i-1]);
                if(2 == poly->size) {
                        const int nr = findSamePointInVecPoint(*poly,p);
                        if(0 <= nr) {
                                //swap with last
                                swapVecPoint(poly, &(inter->poly[inter->size-1]) );
                                return nr;
                        }
                }
        }
        return -1;
}

void appendIntersectionVecPoint(struct Object* obj, struct Object* inter) {
        struct VecPoint *polyOut;
        struct VecPoint *polyIn;
        int size = obj->size;
        int i;
        //you need at least 3 sides for a polygon
        if(3 > inter->size) {
                return;
        }
        //compact inter: remove poly.size != 2 from end on forward
        for(i = inter->size; 0 < i; i--) {
                if(2 == inter->poly[i-1].size) {
                        break;
                }
        }
        objectSetSize(inter,i);
        //you need at least 3 sides for a polygon
        if(3 > inter->size) {
                return;
        }
        //make place for one additional polygon in obj
        objectSetSize(obj,size+1);
        polyOut = &(obj->poly[size]);
        //we have line segments in each poly of inter 
        //take last linesegment as first and second point
        polyIn = &(inter->poly[inter->size-1]);
        append2VecPoint(polyOut,polyIn->points[0]);
        append2VecPoint(polyOut,polyIn->points[1]);
        //remove last poly from inter, because it is already saved
        objectSetSize(inter,inter->size-1);

        //iterate over inter until their is no line segment left
        while(0 < inter->size) {
                //pointer on last point to compare
                Vector3 *lastPt = &(polyOut->points[polyOut->size-1]);
                //find same point in inter to continue polygon
                const int nr = findSamePointInObjectAndSwapWithLast(inter,*lastPt);
                if(0 <= nr) {
                        //last line segment
                        polyIn = &(inter->poly[inter->size-1]);
                        //get the other point in this polygon and save into polyOut
                        append2VecPoint(polyOut,polyIn->points[(nr+1)%2]);
                }
                //remove last poly from inter, because it is already saved or degenerated
                objectSetSize(inter,inter->size-1);
        }
        //last point can be deleted, because he is the same as the first (closes polygon)
        vecPointSetSize(polyOut,polyOut->size-1);
}

double pointPlaneDistance(const struct Plane A, const Vector3 p) {
        return dot(A.n,p)+(A.d);
}

int pointBeforePlane(const struct Plane A, const Vector3 p) {
        return pointPlaneDistance(A,p) > 0.0;
}

int intersectPlaneEdge(Vector3 output, const struct Plane A, const Vector3 a, const Vector3 b) {
        Vector3 diff;
        double t;
        diffVector3(diff,b,a);
        t = dot(A.n,diff);
        if(0.0 == t)
                return 0;
        t = (A.d-dot(A.n,a))/t;
        if(t < 0.0 || 1.0 < t)
                return 0;
        linCombVector3(output,a,diff,t);
        return 1;
}

void clipVecPointByPlane(const struct VecPoint poly, const struct Plane A, struct VecPoint* polyOut, struct VecPoint* interPts) {
        int i;
        int *outside = 0;
        if(poly.size < 3 || 0 == polyOut) {
                return;
        }
        outside = (int*)realloc(outside,sizeof(int)*poly.size);
        //for each point
        for(i = 0; i < poly.size; i++) {
                outside[i] = pointBeforePlane(A,poly.points[i]);
        }
        for(i = 0; i < poly.size; i++) {
                int idNext = (i+1) % poly.size;
                //both outside -> save none
                if(outside[i] && outside[idNext]) {
                        continue;
                }
                //outside to inside -> calc intersection save intersection and save i+1
                if(outside[i]) {
                        Vector3 inter;
                        if(intersectPlaneEdge(inter,A,poly.points[i],poly.points[idNext])) {
                                append2VecPoint(polyOut,inter);
                                if(0 != interPts) {
                                        append2VecPoint(interPts,inter);
                                }
                        }
                        append2VecPoint(polyOut,poly.points[idNext]);
                        continue;
                }
                //inside to outside -> calc intersection save intersection
                if(outside[idNext]) {
                        Vector3 inter;
                        if(intersectPlaneEdge(inter,A,poly.points[i],poly.points[idNext])) {
                                append2VecPoint(polyOut,inter);
                                if(0 != interPts) {
                                        append2VecPoint(interPts,inter);
                                }
                        }
                        continue;
                }
                //both inside -> save point i+1
                append2VecPoint(polyOut,poly.points[idNext]);
        }
        outside = (int*)realloc(outside,0);
}

void clipObjectByPlane(const struct Object obj, const struct Plane A, struct Object* objOut) {
        int i;
        struct Object inter = OBJECT_NULL;
        struct Object objIn = OBJECT_NULL;
        if(0 == obj.size || 0 == objOut)
                return;
        if(obj.poly == objOut->poly) {
                //need to copy object if input and output are the same
                copyObject(&objIn,obj);
        }
        else {
                objIn = obj;
        }
        emptyObject(objOut);

        for(i = 0; i < objIn.size; i++)
        {
                int size = objOut->size;
                objectSetSize(objOut,size+1);
                objectSetSize(&inter,size+1);
                clipVecPointByPlane(objIn.poly[i],A, &(objOut->poly[size]) , &(inter.poly[size]));
                //if whole poly was clipped away -> delete empty poly
                if(0 == objOut->poly[size].size) {
                        objectSetSize(objOut,size);
                        objectSetSize(&inter,size);
                }
        }
        //add a polygon of all intersection points with plane to close the object
        appendIntersectionVecPoint(objOut,&inter);
        emptyObject(&inter);
        emptyObject(&objIn);
}


void clipObjectByAABox(struct Object* obj, const struct AABox box) {
        struct VecPlane planes = VECPLANE_NULL;
        int i;
        if(0 == obj) {
                return;
        }
        
        calcAABoxPlanes(&planes,box);
        for(i = 0; i < planes.size; i++) {
                clipObjectByPlane(*obj,planes.plane[i],obj);
        }

        emptyVecPlane(&planes);
}

int clipTest(const double p, const double q, double* u1, double* u2) {
        // Return value is 'true' if line segment intersects the current test
        // plane.  Otherwise 'false' is returned in which case the line segment
        // is entirely clipped.
        if(0 == u1 || 0 == u2) {
                return 0;
        }
        if(p < 0.0) {
                double r = q/p;
                if(r > (*u2)) {
                        return 0;
                }
                else {
                        if(r > (*u1)) {
                                (*u1) = r;
                        }
                        return 1;
                }
        }
        else {
                if(p > 0.0)     {
                        double r = q/p;
                        if(r < (*u1)) {
                                return 0;
                        }
                        else {
                                if(r < (*u2)) {
                                        (*u2) = r;
                                }
                                return 1;
                        }
                }
                else {
                        return q >= 0.0;
                }
        }
}

int intersectionLineAABox(Vector3 v, const Vector3 p, const Vector3 dir, const struct AABox b) {
        double t1 = 0.0;
        double t2 = DOUBLE_MAX;
        int intersect =
                clipTest(-dir[2],p[2]-b.min[2],&t1,&t2) && clipTest(dir[2],b.max[2]-p[2],&t1,&t2) &&
                clipTest(-dir[1],p[1]-b.min[1],&t1,&t2) && clipTest(dir[1],b.max[1]-p[1],&t1,&t2) &&
                clipTest(-dir[0],p[0]-b.min[0],&t1,&t2) && clipTest(dir[0],b.max[0]-p[0],&t1,&t2);
        if(!intersect) {
                return 0;
        }
        intersect = 0;
        if(0 <= t1) {
                linCombVector3(v,p,dir,t1);
                intersect = 1;
        }
        if(0 <= t2) {
                linCombVector3(v,p,dir,t2);
                intersect = 1;
        }
        return intersect;
}


void includeObjectLightVolume(struct VecPoint* points, const struct Object obj,
                                                          const Vector3 lightDir, const struct AABox sceneAABox) {
        if(0 != points) {
                int i, size;
                Vector3 ld;
                copyVector3Values(ld,-lightDir[0],-lightDir[1],-lightDir[2]);
                convObject2VecPoint(points,obj);
                size = points->size;
                //for each point add the point on the ray in -lightDir
                //intersected with the sceneAABox
                for(i = 0; i < size; i++) {
                        Vector3 pt;
                        if(intersectionLineAABox(pt,points->points[i],ld,sceneAABox) ) {
                                append2VecPoint(points,pt);
                        }
                }
        }
}

void calcFocusedLightVolumePoints(struct VecPoint* points, const Matrix4x4 invEyeProjView,
                                                                  const Vector3 lightDir,const struct AABox sceneAABox) {
        Vector3x8 pts;
        struct Object obj = OBJECT_NULL;

        calcViewFrustumWorldCoord(pts,invEyeProjView);
        calcViewFrustObject(&obj, pts);
        clipObjectByAABox(&obj,sceneAABox);
        includeObjectLightVolume(points,obj,lightDir,sceneAABox);
        emptyObject(&obj);
}

void calcViewFrustumPlanes(struct VecPlane* planes, const Matrix4x4 eyeProjView) {
        if(0 != planes) {
                int i;
                
                planesSetSize(planes, 6);
        
                //---- extract the plane equations
                for (i = 0; i < 4; i++) {
                        setPlaneCoeff(eyeProjView[i * 4 + 3] - eyeProjView[i * 4 + 0], planes->plane + 0, i);   // right plane
                        setPlaneCoeff(eyeProjView[i * 4 + 3] + eyeProjView[i * 4 + 0], planes->plane + 1, i);   // left plane
                        setPlaneCoeff(eyeProjView[i * 4 + 3] + eyeProjView[i * 4 + 1], planes->plane + 2, i);   // bottom plane
                        setPlaneCoeff(eyeProjView[i * 4 + 3] - eyeProjView[i * 4 + 1], planes->plane + 3, i);   // top plane
                        setPlaneCoeff(eyeProjView[i * 4 + 3] - eyeProjView[i * 4 + 2], planes->plane + 4, i);   // far plane
                        setPlaneCoeff(eyeProjView[i * 4 + 3] + eyeProjView[i * 4 + 2], planes->plane + 5, i);   // near plane
                }

                //---- normalize the coefficients
                for (i = 0; i < 6; i++) {
                        float invLength = 1;
                        float length = (float)sqrt(squaredLength(planes->plane[i].n));

                        if(length) invLength    = 1.0f / length;

                        planes->plane[i].n[0] *= invLength;
                        planes->plane[i].n[1] *= invLength;
                        planes->plane[i].n[2] *= invLength;
                        
                        planes->plane[i].d *= invLength;
                }
        }
}

int calcAABNearestVertexIdx(Vector3 clipPlaneNormal) {
        int     index;

        //     7+------+6
        //     /|     /|
        //    / |    / |
        //   / 4+---/--+5
        // 3+------+2 /    y   z
        //  | /    | /     |  /
        //  |/     |/      |/
        // 0+------+1      *---x
        if (clipPlaneNormal[0] <= 0.0f) {
                index   = (clipPlaneNormal[1] <= 0.0f) ? 0 : 3;
        }
        else {
                index   = (clipPlaneNormal[1] <= 0.0f) ? 1 : 2;
        }

        return (clipPlaneNormal[2] <= 0.0f) ? index : 4 + index;
}

int calcAABFarthestVertexIdx(Vector3 clipPlaneNormal) {
        int index;

        if (clipPlaneNormal[0] > 0.0f) {
                index   = (clipPlaneNormal[1] > 0.0f) ? 0 : 3;
        }
        else {
                index   = (clipPlaneNormal[1] > 0.0f) ? 1 : 2;
        }

        return (clipPlaneNormal[2] > 0.0f) ? index : 4 + index;
}

void calcAABNPVertexIndices(int *vertexIndices, const struct VecPlane planes) {
        int i;
        for (i = 0; i < 6; i++) 
        {
                vertexIndices[i * 2 + 0] = calcAABNearestVertexIdx(planes.plane[i].n);  // n-vertex
                vertexIndices[i * 2 + 1] = calcAABFarthestVertexIdx(planes.plane[i].n); // p-vertex
        }
}

void combineAABoxes(struct AABox *a, const struct AABox b) {
        appendToCubicHull(a->min, a->max, b.min);
        appendToCubicHull(a->min, a->max, b.max);
}

double calcAABoxVolume(const struct AABox aab)
{
        return (aab.max[0] - aab.min[0]) * 
                   (aab.max[1] - aab.min[1]) * 
                   (aab.max[2] - aab.min[2]);
}


void calcAABoxCenter(Vector3 vec, const struct AABox aab)
{
        diffVector3(vec, aab.max, aab.min);
        vec[0] *= 0.5f; vec[1] *= 0.5f; vec[2] *= 0.5f;
        
        addVector3(vec, aab.min, vec);
}


double calcAABoxSurface(const struct AABox aab)
{
        return 2 * (aab.max[0] - aab.min[0]) * (aab.max[1] - aab.min[1]) +
                   2 * (aab.max[0] - aab.min[0]) * (aab.max[2] - aab.min[2]) +
                   2 * (aab.max[1] - aab.min[1]) * (aab.max[2] - aab.min[2]);
}

void clipAABoxByAABox(struct AABox *aab, const struct AABox enclosed)
{
        int i;
        for(i=0; i < 3; i++)
        {
                if(aab->min[i] < enclosed.min[i])
                        aab->min[i] = enclosed.min[i];
                if(aab->max[i] > enclosed.max[i])
                        aab->max[i] = enclosed.max[i];
        }
}


void rotateVectorZ(Vector3 v, double rad)
{
        Vector3 temp;

        temp[0] = v[0]*cos(rad) - v[1]*sin(rad);
        temp[1] = v[0]*sin(rad) + v[1]*cos(rad);
        temp[2] = v[2];

        copyVector3(v, temp);
}


void rotateVectorX(Vector3 v, double rad)
{
        Vector3 temp;
        
        temp[0] = v[0];
        temp[1] = v[1]*cos(rad) - v[2]*sin(rad);
        temp[2] = v[1]*sin(rad) + v[2]*cos(rad);
        
        copyVector3(v, temp);
}


void rotateVectorY(Vector3 v, double rad)
{
        Vector3 temp;

        temp[0] = v[2]*sin(rad) + v[0]*cos(rad);
        temp[1] = v[1]; 
        temp[2] = v[2]*cos(rad) - v[0]*sin(rad);

        copyVector3(v, temp);
}



void rotateVector(Vector3 result, const Vector3 input, const float rad, const Vector3 axis)
{
        // using quaternions (vector and scalar part)
        float s = (float)cos(rad * 0.5);
        float h = (float)sin(rad * 0.5);
        float p_v_dot;

        Vector3 hvec = {axis[0] * h, axis[1]* h, axis[2] * h};
        Vector3 hvec2;
        Vector3 hvec3;

        cross(hvec2, hvec, input);
        cross(hvec3, hvec, hvec2);

        p_v_dot = (float)dot(input, hvec);

        result[0] = s * s * input[0] + hvec[0] * p_v_dot + 
                                         2.0f * s * hvec2[0] + hvec3[0];

        result[1] = s * s * input[1] + hvec[1] * p_v_dot + 
                                         2.0f * s *hvec2[1] + hvec3[1];

        result[2] = s * s * input[2] + hvec[2] * p_v_dot + 
                                         2.0f * s *hvec2[2] + hvec3[2];

}


void vectorNormal(Vector3 result, const Vector3 vec)
{
        result[0] = 0.0f;
        result[1] = 0.0f;
        result[2] = 0.0f;

        // to avoid 0-vector: look if at least one component is not 0)
        if((vec[0] != 0.0f) || (vec[1] != 0.0f))
        {
                result[0] = -vec[1];
                result[1] = vec[0];
        }       
        else
        {
                result[0] = -vec[2];
                result[1] = vec[0];
        }
}