src/bezier.cpp

Go to the documentation of this file.

/***************************************************************************
 *            bezier.cc
 *
 *  Sat Nov 12 10:20:06 2005
 *  Copyright  2005  Joe Venzon
 *  joe@venzon.net
 ****************************************************************************/

/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */
 
#include "bezier.h"

BEZIER::BEZIER()
{
        int x,y;
        
        for (x = 0; x < 4; x++)
        {
                for (y = 0; y < 4; y++)
                {
                        points[x][y].zero();
                }
        }
}

BEZIER::~BEZIER()
{
        
}

void BEZIER::SetFromCorners(VERTEX fl, VERTEX fr, VERTEX bl, VERTEX br)
{
        VERTEX temp;
        
        center = fl + fr + bl + br;
        center.Scale(0.25);
        radius = 0;
        if ((fl - center).len() > radius)
                radius = (fl - center).len();
        if ((fr - center).len() > radius)
                radius = (fr - center).len();
        if ((bl - center).len() > radius)
                radius = (bl - center).len();
        if ((br - center).len() > radius)
                radius = (br - center).len();
        
        //assign corners
        points[0][0] = fl;
        points[0][3] = fr;
        points[3][3] = br;
        points[3][0] = bl;
        
        //calculate intermediate front and back points
        
        temp = fr - fl;
        points[0][1] = fl + temp.normalize().ScaleR(temp.len()/3.0);
        points[0][2] = fl + temp.normalize().ScaleR(2.0*temp.len()/3.0);
        
        temp = br - bl;
        points[3][1] = bl + temp.normalize().ScaleR(temp.len()/3.0);
        points[3][2] = bl + temp.normalize().ScaleR(2.0*temp.len()/3.0);
        
        
        //calculate intermediate left and right points  
        int i;
        for (i = 0; i < 4; i++)
        {
                temp = points[3][i] - points[0][i];
                points[1][i] = points[0][i] + temp.normalize().ScaleR(temp.len()/3.0);
                points[2][i] = points[0][i] + temp.normalize().ScaleR(2.0*temp.len()/3.0);
        }
}

void BEZIER::Visualize(bool wireframe, bool fill, VERTEX color)
{
        if (!fill && !wireframe)
                return;
        
        glPushAttrib(GL_ALL_ATTRIB_BITS);
        
        glDisable(GL_LIGHTING);
        glDisable(GL_TEXTURE_2D);
        
        if (fill)
        {
                glColor4f(1,1,1,1);
                glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
                //glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
                glDisable(GL_DEPTH_TEST);
                glEnable(GL_BLEND);
                glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
                float trans = 0.25;
                glColor4f(1,1,1,trans);
                DrawSurf(SURFDRAW_VIS, trans);
                glEnable(GL_DEPTH_TEST);
        }
        
        if (wireframe)
        {
                glDisable(GL_DEPTH_TEST);
                glLineWidth(1.0);
                glEnable(GL_BLEND);
                glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
                //glColor4f(.5,.6,.5,0.1);
                glColor4f(color.x, color.y, color.z,0.1);
                DrawControlPoints();
                
                glEnable(GL_DEPTH_TEST);
                glDisable(GL_BLEND);
                glLineWidth(2.0);
                glColor4f(color.x, color.y, color.z,1);
                DrawControlPoints();
        }
        
        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        glPopAttrib();
}

void BEZIER::DrawSurf(int div, float trans)
{
        int x, y;
        
        float px, py, pxo, pyo, col;
        
        //VERTEX temp[4];
        VERTEX normal;
        
        if (div < 1)
                div = 1;
        
        glBegin(GL_TRIANGLES);
        
        for (x = 0; x < div; x++)
        {
                for (y = 0; y < div; y++)
                {
                        //if (x < div - 1 && y < div - 1)
                        {
                                px = (float) x / (float) div;
                                py = (float) y / (float) div;
                                pxo = (float) (x+1) / (float) div;
                                pyo = (float) (y+1) / (float) div;
                                
                                normal = (SurfCoord(pxo, pyo) - SurfCoord(px, py)).cross(SurfCoord(pxo, py) - SurfCoord(px, py));
                                normal = normal.normalize();
                                col = normal.y;
                                col = col * col;
                                glColor4f(col,col,col, trans);
                                
                                glVertex3fv(SurfCoord(pxo, py).v3());
                                glVertex3fv(SurfCoord(px, py).v3());
                                glVertex3fv(SurfCoord(px, pyo).v3());
                                
                                normal = (SurfCoord(px, pyo) - SurfCoord(px, py)).cross(SurfCoord(pxo, pyo) - SurfCoord(px, py));
                                normal = normal.normalize();
                                col = normal.y;
                                col = col * col;
                                glColor4f(col,col,col, trans);
                                
                                glVertex3fv(SurfCoord(px, pyo).v3());
                                glVertex3fv(SurfCoord(pxo, pyo).v3());
                                glVertex3fv(SurfCoord(pxo, py).v3());
                        }
                }
        }

        glEnd();
}

void BEZIER::DrawControlPoints()
{
        glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

        //glColor4f(1,0,0,1);
        
        /*int x, y;
        glPointSize(5.0);
        glBegin(GL_POINTS);
        
        for (x = 0; x < 4; x++)
        {
                for (y = 0; y < 4; y++)
                {
                        glVertex3fv(points[x][y].v3());
                }
        }

        glEnd();*/
        
        //glColor4f(0,1,0,1);
        
        glBegin(GL_QUADS);
        
        glVertex3fv(points[0][0].v3());
        glVertex3fv(points[3][0].v3());
        glVertex3fv(points[3][3].v3());
        glVertex3fv(points[0][3].v3());

        glEnd();
        
        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}

void BEZIER::Attach(BEZIER & other)
{
        int x;
        
        //move the other patch to the location of this patch
        /*for (x = 0; x < 4; x++)
                points[0][x] = other.points[3][x];*/
        
        //move the other patch to the location of this patch and force its
        // intermediate points into a nice grid layout
        other.SetFromCorners(other.points[0][0], other.points[0][3], points[0][0], points[0][3]);
        
        VERTEX slope;
        
        for (x = 0; x < 4; x++)
        {
                //slope points in the forward direction
                slope = (other.points[0][x] - points[3][x]).normalize();
                
                float otherlen = (other.points[0][x] - other.points[3][x]).len();
                float mylen = (points[0][x] - points[3][x]).len();
                
                float meanlen = (otherlen + mylen)/2.0;
                float leglen = meanlen / 3.0;
                
                other.points[2][x] = other.points[3][x] + slope.ScaleR(leglen);
                points[1][x] = points[0][x] + slope.ScaleR(-leglen);
        }
}

VERTEX BEZIER::Bernstein(float u, VERTEX *p)
{
        VERTEX  a, b, c, d, r;

        a = p[0].ScaleR(pow(u,3));
        b = p[1].ScaleR(3*pow(u,2)*(1-u));
        c = p[2].ScaleR(3*u*pow((1-u),2));
        d = p[3].ScaleR(pow((1-u),3));

        //r = pointAdd(pointAdd(a, b), pointAdd(c, d));
        r = a+b+c+d;

        return r;
}

VERTEX BEZIER::SurfCoord(float px, float py)
{
        VERTEX temp[4];
        VERTEX temp2[4];
        int i, j;

        /*if (px == 0.0 && py == 0.0)
                return points[3][3];
        if (px == 1.0 && py == 1.0)
                return points[0][0];
        if (px == 1.0 && py == 0.0)
                return points[0][3];
        if (px == 0.0 && py == 1.0)
                return points[3][0];*/
        
        //get splines along x axis
        for (j = 0; j < 4; j++)
        {
                for (i = 0; i < 4; i++)
                        temp2[i] = points[j][i];
                temp[j] = Bernstein(px, temp2);
        }
        
        return Bernstein(py, temp);
}

VERTEX BEZIER::SurfNorm(float px, float py)
{
        VERTEX temp[4];
        VERTEX temp2[4];
        VERTEX tempx[4];
        int i, j;

        //get splines along x axis
        for (j = 0; j < 4; j++)
        {
                for (i = 0; i < 4; i++)
                        temp2[i] = points[j][i];
                temp[j] = Bernstein(px, temp2);
        }
        
        //get splines along y axis
        for (j = 0; j < 4; j++)
        {
                for (i = 0; i < 4; i++)
                        temp2[i] = points[i][j];
                tempx[j] = Bernstein(py, temp2);
        }
        
        //return BernsteinTangent(py, temp);
        return BernsteinTangent(px, tempx).cross(BernsteinTangent(py, temp)).normalize().InvertR();
}

VERTEX BEZIER::BernsteinTangent(float u, VERTEX *p)
{
        VERTEX  a, b, c, r;

        a = (p[1]-p[0]).ScaleR(3*pow(u,2));
        b = (p[2]-p[1]).ScaleR(3*2*u*(1-u));
        c = (p[3]-p[2]).ScaleR(3*pow((1-u),2));

        //r = pointAdd(pointAdd(a, b), pointAdd(c, d));
        r = a+b+c;

        return r;
}

void BEZIER::GetTri(int div, int num, VERTEX outtri[3])
{
        int firsttri = num % 2;
        
        int x = (num/2) % div;
        int y = (num/2) / div;

        if (firsttri == 0)
        {               
                float px = (float) x / (float) div;
                float py = (float) y / (float) div;
                float pxo = (float) (x+1) / (float) div;
                float pyo = (float) (y+1) / (float) div;
        
                /*glVertex3fv(SurfCoord(px, py).v3());
                glVertex3fv(SurfCoord(pxo, py).v3());
                glVertex3fv(SurfCoord(pxo, pyo).v3());*/
                
                outtri[0] = SurfCoord(px, py);
                outtri[1] = SurfCoord(pxo, py);
                outtri[2] = SurfCoord(px, pyo);
        }
        else
        {
                float px = (float) x / (float) div;
                float py = (float) y / (float) div;
                float pxo = (float) (x+1) / (float) div;
                float pyo = (float) (y+1) / (float) div;
                
                /*glVertex3fv(SurfCoord(px, py).v3());
                glVertex3fv(SurfCoord(pxo, pyo).v3());
                glVertex3fv(SurfCoord(px, pyo).v3());*/
                
                outtri[0] = SurfCoord(pxo, pyo);
                outtri[1] = SurfCoord(px, pyo);
                outtri[2] = SurfCoord(pxo, py);
        }
}

bool BEZIER::CollideSubDiv(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
        int i;
        
        VERTEX curtri[3];
        
        int retval = 0;
        
        float t, u, v;
        
        int x, y;
        
        for (i = 0; i < NumTris(COLLISION_DIVS) && !retval; i++)
        {       
                GetTri(COLLISION_DIVS, i, curtri);
                
                retval = INTERSECT_FUNCTION(origin.v3(), direction.v3(),
                        curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
                        &t, &u, &v);
                
                if (retval)
                {
                        if (i % 2 == 0)
                        {
                                u = 1.0 - u;
                                v = 1.0 - v;
                        }
                        
                        u = 1.0 - u;
                        v = 1.0 - v;
                        
                        x = (i/2) % COLLISION_DIVS;
                        y = (i/2) / COLLISION_DIVS;
                        u += (float) x;
                        v += (float) y;
                        u = u / (float) COLLISION_DIVS;
                        v = v / (float) COLLISION_DIVS;
                        
                        //u = 1.0 - u;
                        //v = 1.0 - v;
                        
                        //cout << u << "," << v << endl;
                        outtri = SurfCoord(u, v);
                        //outtri = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
                        return true;
                }
        }
        
        outtri = origin;
        return false;
}

bool BEZIER::CollideNewton(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
        int i;
        
        VERTEX curtri[3];
        VERTEX coltri[3];
        
        int retval = 0;
        bool collided = false;
        
        float su, sv, tu, tv;
        su = sv = tu = tv = 0;
        
        float t, u, v;
        
        int x, y;
        int j;
        
        for (i = 0; i < NumTris(1) && !retval; i++)
        {       
                GetTri(COLLISION_DIVS, i, curtri);
                
                retval = INTERSECT_FUNCTION(origin.v3(), direction.v3(),
                        curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
                        &t, &u, &v);
                
                if (retval)
                {
                        for (j = 0; j < 3; j++)
                                coltri[j] = curtri[j];
                        tu = u;
                        tv = v;
                        
                        if (i % 2 == 0)
                        {
                                u = 1.0 - u;
                                v = 1.0 - v;
                        }
                        
                        u = 1.0 - u;
                        v = 1.0 - v;
                        
                        x = (i/2) % COLLISION_DIVS;
                        y = (i/2) / COLLISION_DIVS;
                        u += (float) x;
                        v += (float) y;
                        u = u / (float) COLLISION_DIVS;
                        v = v / (float) COLLISION_DIVS;
                        
                        //u = 1.0 - u;
                        //v = 1.0 - v;
                        
                        //cout << u << "," << v << endl;
                        outtri = SurfCoord(u, v);
                        su = u;
                        sv = v;
                        //return true;
                        collided = true;
                }
        }
        
        if (collided)
        {
                bool loop = true;
                bool verbose = false;
                VERTEX normal;
                VERTEX approx;
                VERTEX actual;
                VERTEX guess = outtri;
                VERTEX correct;
                MATRIX3 proj;
                
                float error, lasterror;
                
                approx = curtri[0].ScaleR(1-tu-tv) + curtri[1].ScaleR(tu) + curtri[2].ScaleR(tv);
                
                float spanu = (points[0][3] - points[0][0]).len();
                float spanv = (points[3][0] - points[0][0]).len();
                
                //error = (approx - outtri).len();
                //error is the distance from direction vector to actual position
                VERTEX dirnorm = direction.normalize();
                VERTEX errorvec;
                
                errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
                error = errorvec.len();
                lasterror = error;
                
                if (verbose)
                        cout << error << endl;
                
                float uit = (error / spanu)/2.0;
                float vit = (error / spanv)/2.0;
                float nu, nv;
                
                float biasv, biasu;
                biasu = biasv = 1.0;
                
                float erroru, errorv;
                
                nu = su + uit;
                nv = sv + vit;
                if (nu > 1.0)
                        nu = 1.0;
                if (nv > 1.0)
                        nv = 1.0;
                guess = SurfCoord(nu,sv);
                errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
                error = errorvec.len();
                erroru = error - lasterror;
                if (error > lasterror)
                        biasu = -1.0;
                if (verbose) cout << error << endl;
                guess = SurfCoord(su,nv);
                errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
                error = errorvec.len();
                errorv = error - lasterror;
                if (error > lasterror)
                        biasv = -1.0;
                if (verbose) cout << error << endl;
                        
                biasu *= erroru/(erroru+errorv);
                biasv *= errorv/(erroru+errorv);
                
                VERTEX lastguess = guess;
                
                nu = su;
                nv = sv;
                
                float ou, ov;
                ou = nu;
                ov = nv;
                
                VERTEX tempguess;
                
                int n;
                for (n = 0; (n < 20) && loop; n++)
                {
                        lastguess = guess;
                        ou = nu;
                        ov = nv;
                        
                        nu += biasu * (error / spanu)/1.0;
                        nv += biasv * (error / spanv)/1.0;
                        
                        tempguess = SurfCoord(nu, ov);
                        if (((tempguess - origin) - dirnorm.ScaleR((tempguess - origin).dot(dirnorm))).len() > lasterror)
                        {
                                biasu = 0.0;
                                nu = ou;
                        }
                        tempguess = SurfCoord(ou, nv);
                        if (((tempguess - origin) - dirnorm.ScaleR((tempguess - origin).dot(dirnorm))).len() > lasterror)
                        {
                                biasv = 0.0;
                                nv = ov;
                        }
                        
                        guess = SurfCoord(nu, nv);
                        
                        lasterror = error;
                        errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
                        error = errorvec.len();
                        if (verbose) cout << n << ": " << error << endl;
                        
                        if (lasterror <= error || biasu == 0.0 && biasv == 0.0)
                                loop = false;
                }
                
                /*int n;
                for (n = 0; (n < 1) && loop; n++)
                {
                        cout << n << ": " << error << endl;
                        
                        actual = SurfCoord(su, sv);
                        normal = SurfNorm(su, sv);
                        proj.ProjectionMatrix(normal.normalize());
                        correct = approx - actual;
                        guess = proj.Multiply(correct);
                        
                        lasterror = error;
                        errorvec = dirnorm.ScaleR((outtri - origin).dot(dirnorm));
                        error = errorvec.len();
                }*/
                
                /*approx.DebugPrint();
                actual.DebugPrint();
                normal.DebugPrint();
                //proj.DebugPrint();
                guess.DebugPrint();
                guess = guess + actual;
                guess.DebugPrint();*/
                //cout << n << ": " << (guess - outtri).len() << endl;
                if (verbose) cout << endl;
                
                outtri = lastguess;     
                
                return true;
        }
        
        outtri = origin;
        return false;
}

void BEZIER::CopyFrom(BEZIER &other)
{
        int x, y;
        
        for (x = 0; x < 4; x++)
        {
                for (y = 0; y < 4; y++)
                {
                        points[x][y] = other.points[x][y];
                }
        }
}

bool BEZIER::ReadFrom(ifstream &openfile)
{
        int x, y;
        
        if (!openfile)
                return false;
        
        for (x = 0; x < 4; x++)
        {
                for (y = 0; y < 4; y++)
                {
                        openfile >> points[x][y].x;
                        openfile >> points[x][y].y;
                        openfile >> points[x][y].z;
                }
        }
        
        return true;
}

bool BEZIER::WriteTo(ofstream &openfile)
{
        int x, y;
        
        for (x = 0; x < 4; x++)
        {
                for (y = 0; y < 4; y++)
                {
                        openfile << points[x][y].x << " ";
                        openfile << points[x][y].y << " ";
                        openfile << points[x][y].z << endl;
                }
        }
        
        return true;
}

bool BEZIER::CollideSingleQuad(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
        bool col = false;
        
        float t, u, v;
        
        col = INTERSECT_QUAD_FUNCTION(origin, direction,
                points[0][0], points[0][3], points[3][3], points[3][0],
                t, u, v);
        
        if (col)
        {
                /*if (i % 2 == 0)
                {
                        u = 1.0 - u;
                        v = 1.0 - v;
                }
                
                u = 1.0 - u;
                v = 1.0 - v;
                
                x = (i/2) % COLLISION_DIVS;
                y = (i/2) / COLLISION_DIVS;
                u += (float) x;
                v += (float) y;
                u = u / (float) COLLISION_DIVS;
                v = v / (float) COLLISION_DIVS;*/
                
                u = 1.0 - u;
                v = 1.0 - v;
                
                //cout << u << "," << v << endl;
                outtri = SurfCoord(u, v);
                //outtri = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
                return true;
        }
        
        outtri = origin;
        return false;
}

bool BEZIER::CollideQuadNewton(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
        bool col = false;
        
        float t, u, v;
        
        col = INTERSECT_QUAD_FUNCTION(origin, direction,
                points[0][0], points[0][3], points[3][3], points[3][0],
                t, u, v);
        
        if (col)
        {
                /*if (i % 2 == 0)
                {
                        u = 1.0 - u;
                        v = 1.0 - v;
                }
                
                u = 1.0 - u;
                v = 1.0 - v;
                
                x = (i/2) % COLLISION_DIVS;
                y = (i/2) / COLLISION_DIVS;
                u += (float) x;
                v += (float) y;
                u = u / (float) COLLISION_DIVS;
                v = v / (float) COLLISION_DIVS;*/
                
                u = 1.0 - u;
                v = 1.0 - v;
                
                //cout << u << "," << v << endl;
                outtri = SurfCoord(u, v);
                float su = u;
                float sv = v;
                //outtri = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
                
                bool loop = true;
                bool verbose = true;
                VERTEX normal;
                VERTEX approx;
                VERTEX actual;
                VERTEX guess = outtri;
                VERTEX correct;
                MATRIX3 proj;
                
                float error, lasterror;
                
                float spanu = (points[0][3] - points[0][0]).len();
                float spanv = (points[3][0] - points[0][0]).len();
                
                //error = (approx - outtri).len();
                //error is the distance from direction vector to actual position
                VERTEX dirnorm = direction.normalize();
                VERTEX errorvec;
                
                errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
                error = errorvec.len();
                lasterror = error;
                
                if (verbose)
                        cout << error << endl;
                
                float uit = (error / spanu)/2.0;
                float vit = (error / spanv)/2.0;
                float nu, nv;
                
                float biasv, biasu;
                biasu = biasv = 1.0;
                
                float erroru, errorv;
                
                nu = su + uit;
                nv = sv + vit;
                if (nu > 1.0)
                        nu = 1.0;
                if (nv > 1.0)
                        nv = 1.0;
                guess = SurfCoord(nu,sv);
                errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
                error = errorvec.len();
                erroru = error - lasterror;
                if (error > lasterror)
                        biasu = -1.0;
                if (verbose) cout << error << endl;
                guess = SurfCoord(su,nv);
                errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
                error = errorvec.len();
                errorv = error - lasterror;
                if (error > lasterror)
                        biasv = -1.0;
                if (verbose) cout << error << endl;
                        
                biasu *= erroru/(erroru+errorv);
                biasv *= errorv/(erroru+errorv);
                
                VERTEX lastguess = guess;
                
                nu = su;
                nv = sv;
                
                float ou, ov;
                ou = nu;
                ov = nv;
                
                VERTEX tempguess;
                
                int n;
                for (n = 0; (n < 5) && loop; n++)
                {
                        lastguess = guess;
                        ou = nu;
                        ov = nv;
                        
                        nu += biasu * (error / spanu)/1.0;
                        nv += biasv * (error / spanv)/1.0;
                        
                        tempguess = SurfCoord(nu, ov);
                        if (((tempguess - origin) - dirnorm.ScaleR((tempguess - origin).dot(dirnorm))).len() > lasterror)
                        {
                                biasu = 0.0;
                                nu = ou;
                        }
                        tempguess = SurfCoord(ou, nv);
                        if (((tempguess - origin) - dirnorm.ScaleR((tempguess - origin).dot(dirnorm))).len() > lasterror)
                        {
                                biasv = 0.0;
                                nv = ov;
                        }
                        
                        guess = SurfCoord(nu, nv);
                        
                        lasterror = error;
                        errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
                        error = errorvec.len();
                        if (verbose) cout << n << ": " << error << endl;
                        
                        if (lasterror <= error || biasu == 0.0 && biasv == 0.0)
                                loop = false;
                }
                
                /*int n;
                for (n = 0; (n < 1) && loop; n++)
                {
                        cout << n << ": " << error << endl;
                        
                        actual = SurfCoord(su, sv);
                        normal = SurfNorm(su, sv);
                        proj.ProjectionMatrix(normal.normalize());
                        correct = approx - actual;
                        guess = proj.Multiply(correct);
                        
                        lasterror = error;
                        errorvec = dirnorm.ScaleR((outtri - origin).dot(dirnorm));
                        error = errorvec.len();
                }*/
                
                /*approx.DebugPrint();
                actual.DebugPrint();
                normal.DebugPrint();
                //proj.DebugPrint();
                guess.DebugPrint();
                guess = guess + actual;
                guess.DebugPrint();*/
                //cout << n << ": " << (guess - outtri).len() << endl;
                if (verbose) cout << endl;
                
                outtri = lastguess;     
                
                return true;
        }
        
        outtri = origin;
        return false;   
}

bool BEZIER::CollideSubDivQuad(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
        bool col = false;
        
        const bool verbose = false;
        
        const bool projhack = true;
        
        if (projhack)
                origin.y = 1;
        
        float t, u, v;
        
        float su = 0;
        float sv = 0;
        
        float umin = 0;
        float umax = 1;
        float vmin = 0;
        float vmax = 1;
        
        /*VERTEX ul = points[0][0];
        VERTEX ur = points[0][3];
        VERTEX br = points[3][3];
        VERTEX bl = points[3][0];*/
        
        VERTEX ul = points[3][3];
        VERTEX ur = points[3][0];
        VERTEX br = points[0][0];
        VERTEX bl = points[0][3];
        
        int subdivnum = 0;

        int i;
        for (i = 0; i < COLLISION_QUAD_DIVS; i++)
        {       
                //speedup for i == 0
                if (i != 0)
                {
                        ul = SurfCoord(umin, vmin);
                        ur = SurfCoord(umax, vmin);
                        br = SurfCoord(umax, vmax);
                        bl = SurfCoord(umin, vmax);
                }
                
                if (projhack)
                {
                        ul.y = ur.y = br.y = bl.y = 0;
                }
                
                //u = v = 0.0;

                col = INTERSECT_QUAD_FUNCTION(origin, direction,
                        ul, ur, br, bl,
                        t, u, v);
                
                if (col)
                {
                        //correct UV
                        //u = 1.0 - u;
                        //v = 1.0 - v;
                        
                        //expand quad UV to surface UV
                        su = u * (umax - umin) + umin;
                        sv = v * (vmax - vmin) + vmin;
                        
                        //calculate error
                        if (verbose)
                        {
                                VERTEX guess = SurfCoord(su, sv);
                                VERTEX dirnorm = direction.normalize();
                                VERTEX errorvec;
                                
                                errorvec = (guess - origin) - dirnorm.ScaleR((guess - origin).dot(dirnorm));
                                
                                cout << i << ": " << errorvec.len() << endl;
                        }
                        
                        //evaluate which quadrant was hit
                        if (v <= 0.5)
                        {
                                //top side
                                vmax = 0.5*(vmax-vmin)+vmin;
                                subdivnum = 0;
                        }
                        else
                        {
                                //bottom side
                                vmin = 0.5*(vmax-vmin)+vmin;
                                subdivnum = 2;
                        }
                        
                        if (u <= 0.5)
                        {
                                //left side
                                umax = 0.5*(umax-umin) + umin;
                        }
                        else
                        {
                                //right side
                                umin = 0.5*(umax-umin) + umin;
                                subdivnum++;
                        }
                        
                        //outtri = SurfCoord(u, v);
                }
                else
                {
                        if (verbose)
                                cout << i << ": nocol " << origin.x << endl;
                        
                        if ((i == 0) && QUAD_DIV_FAST_DISCARD)
                        //if (QUAD_DIV_FAST_DISCARD)
                        {
                                outtri = origin;
                                return false;
                        }
                        else
                        {
                                //need to try again with a different subdiv
                                
                                //undo previous changes to uv rect
                                float umin0 = umin;
                                float umax0 = umax;
                                float vmin0 = vmin;
                                float vmax0 = vmax;
                                
                                if (subdivnum < 2)
                                        vmax0 = (vmax-vmin)*2.0 + vmin;
                                else
                                        vmin0 = vmax - (vmax-vmin)*2.0;
                                
                                if (subdivnum % 2 == 0)
                                        umax0 = (umax-umin)*2.0 + umin;
                                else
                                        umin0 = umax - (umax-umin)*2.0;
                                
                                if (verbose)
                                {
                                        cout << endl << "Guessed wrong on " << subdivnum << endl;
                                        cout << umin << ", " << umax << " : " << vmin << ", " << vmax << endl;
                                        cout << umin0 << ", " << umax0 << " : " << vmin0 << ", " << vmax0 << endl;
                                }
                                
                                if (verbose)
                                {
                                        cout << "!" << subdivnum << endl;
                                        ul.DebugPrint();
                                        ur.DebugPrint();
                                        br.DebugPrint();
                                        bl.DebugPrint();
                                        cout << endl;
                                }
                                
                                //try other rects
                                int s;
                                bool scol = false;
                                int curnum = 0;
                                for (s = 1; s < 4 && !scol; s++)
                                {
                                        curnum = (4 + subdivnum - s) % 4;
                                        
                                        vmin = vmin0;
                                        vmax = vmax0;
                                        umin = umin0;
                                        umax = umin0;
                                        
                                        if (curnum < 2)
                                                vmax = 0.5*(vmax0-vmin0)+vmin0;
                                        else
                                                vmin = 0.5*(vmax0-vmin0)+vmin0;
                                        
                                        if (curnum % 2 == 0)
                                                umax = 0.5*(umax0-umin0) + umin0;
                                        else
                                                umin = 0.5*(umax0-umin0) + umin0;
                                        
                                        if (verbose)
                                        {
                                                cout << curnum << endl;
                                                cout << umin0 << ", " << umax0 << " : " << vmin0 << ", " << vmax0 << endl;
                                                cout << umin << ", " << umax << " : " << vmin << ", " << vmax << endl;
                                        }

                                        ul = SurfCoord(umin, vmin);
                                        ur = SurfCoord(umax, vmin);
                                        br = SurfCoord(umax, vmax);
                                        bl = SurfCoord(umin, vmax);
                                        
                                        if (projhack)
                                        {
                                                ul.y = ur.y = br.y = bl.y = 0;
                                        }
                                        
                                        if (verbose)
                                        {
                                                ul.DebugPrint();
                                                ur.DebugPrint();
                                                br.DebugPrint();
                                                bl.DebugPrint();
                                                cout << endl;
                                        }
                        
                                        scol = INTERSECT_QUAD_FUNCTION(origin, direction,
                                                ul, ur, br, bl,
                                                t, u, v);
                                        
                                        if (scol)
                                        {
                                                //rejoice!
                                                su = u * (umax - umin) + umin;
                                                sv = v * (vmax - vmin) + vmin;
                                                
                                                //evaluate which quadrant was hit
                                                if (v <= 0.5)
                                                {
                                                        //top side
                                                        vmax = 0.5*(vmax-vmin)+vmin;
                                                        subdivnum = 0;
                                                }
                                                else
                                                {
                                                        //bottom side
                                                        vmin = 0.5*(vmax-vmin)+vmin;
                                                        subdivnum = 2;
                                                }
                                                
                                                if (u <= 0.5)
                                                {
                                                        //left side
                                                        umax = 0.5*(umax-umin) + umin;
                                                }
                                                else
                                                {
                                                        //right side
                                                        umin = 0.5*(umax-umin) + umin;
                                                        subdivnum++;
                                                }
                                        }
                                }
                                
                                if (verbose)
                                {
                                        if (!scol)
                                        {
                                                cout << "Never found poper quad. " << origin.x << endl;
                                                //origin.DebugPrint();
                                                //direction.DebugPrint();
                                                //cout << endl;
                                        }
                                        //else
                                                //cout << "Found proper quad after " << s-1 << " trie(s). " << origin.x << endl;
                                }
                                
                                if (!scol)
                                {
                                        //cry
                                        
                                        //outtri = origin;
                                        //return false;
                                }
                        }
                        
                        /*if (i != 0)
                        {
                                loop = false;
                                u = oldu;
                                v = oldv;
                        }*/
                }
        }
        
        if (verbose)
                cout << endl;
        
        /*col = INTERSECT_QUAD_FUNCTION(origin, direction,
                points[0][0], points[0][3], points[3][3], points[3][0],
                t, u, v);
        
        if (col)
        {       
                u = 1.0 - u;
                v = 1.0 - v;
                
                //cout << u << "," << v << endl;
                outtri = SurfCoord(u, v);
                return true;
        }*/
        
        if (col || QUAD_DIV_FAST_DISCARD)
        //if (col)
        {
                if (!col)
                        cout << "blip " << su << "," << sv << ": " << u << "," << v << endl;
                outtri = SurfCoord(su, sv);
                return true;
        }
        else
        {
                outtri = origin;
                return false;
        }       
}

bool BEZIER::CollideSubDivQuadSimple(VERTEX origin, VERTEX direction, VERTEX &outtri)
{
        VERTEX normal;
        return CollideSubDivQuadSimpleNorm(origin, direction, outtri, normal);
}

bool BEZIER::CollideSubDivQuadSimpleNorm(VERTEX origin, VERTEX direction, VERTEX &outtri, VERTEX & normal)
{
        bool col = false;
        
        const bool verbose = false;
        //const bool verbose = true;
        
        const bool projhack = false;
        
        if (projhack)
                origin.y = 1;
        
        float t, u, v;
        
        float su = 0;
        float sv = 0;
        
        float umin = 0;
        float umax = 1;
        float vmin = 0;
        float vmax = 1;
        
        //const float fuzziness = 0.13;
        
        /*VERTEX ul = points[0][0];
        VERTEX ur = points[0][3];
        VERTEX br = points[3][3];
        VERTEX bl = points[3][0];*/
        
        VERTEX ul = points[3][3];
        VERTEX ur = points[3][0];
        VERTEX br = points[0][0];
        VERTEX bl = points[0][3];
        
        //int subdivnum = 0;

        bool loop = true;
        
        float areacut = 0.5;

        int i;
        for (i = 0; i < COLLISION_QUAD_DIVS && loop; i++)
        {
                float tu[2];
                float tv[2];
                
                //speedup for i == 0
                //if (i != 0)
                {
                        tu[0] = umin;
                        if (tu[0] < 0)
                                tu[0] = 0;
                        tu[1] = umax;
                        if (tu[1] > 1)
                                tu[1] = 1;
                        
                        tv[0] = vmin;
                        if (tv[0] < 0)
                                tv[0] = 0;
                        tv[1] = vmax;
                        
                        if (tv[1] > 1)
                                tv[1] = 1;
                        
                        ul = SurfCoord(tu[0], tv[0]);
                        ur = SurfCoord(tu[1], tv[0]);
                        br = SurfCoord(tu[1], tv[1]);
                        bl = SurfCoord(tu[0], tv[1]);
                        
                        /*umin = u[0];
                        umax = v[1];
                        vmin = v[0];
                        vmax = v[1];*/
                        
                        /*ul = SurfCoord(umin, vmin);
                        ur = SurfCoord(umax, vmin);
                        br = SurfCoord(umax, vmax);
                        bl = SurfCoord(umin, vmax);*/
                }
                
                if (projhack)
                {
                        ul.y = ur.y = br.y = bl.y = 0;
                }
                
                //u = v = 0.0;

                col = INTERSECT_QUAD_FUNCTION(origin, direction,
                        ul, ur, br, bl,
                        t, u, v);
                
                if (col)
                {
                        //expand quad UV to surface UV
                        //su = u * (umax - umin) + umin;
                        //sv = v * (vmax - vmin) + vmin;
                        
                        su = u * (tu[1] - tu[0]) + tu[0];
                        sv = v * (tv[1] - tv[0]) + tv[0];
                        
                        //place max and min according to area hit
                        vmax = sv + (0.5*areacut)*(vmax-vmin);
                        vmin = sv - (0.5*areacut)*(vmax-vmin);
                        umax = su + (0.5*areacut)*(umax-umin);
                        umin = su - (0.5*areacut)*(umax-umin);
                        
                        /*//evaluate which quadrant was hit
                        if (v <= 0.5)
                        {
                                //top side
                                vmax = (0.5+fuzziness)*(vmax-vmin)+vmin;
                                //vmax = (0.5+fuzziness)*(vmax-vmin)+vmin-fuzziness*0.5;
                                //vmin -= fuzziness*0.5;
                                subdivnum = 0;
                        }
                        else
                        {
                                //bottom side
                                vmin = (0.5-fuzziness)*(vmax-vmin)+vmin;
                                subdivnum = 2;
                        }
                        
                        if (u <= 0.5)
                        {
                                //left side
                                umax = (0.5+fuzziness)*(umax-umin) + umin;
                        }
                        else
                        {
                                //right side
                                umin = (0.5-fuzziness)*(umax-umin) + umin;
                                subdivnum++;
                        }*/
                }
                else
                {
                        if ((i == 0) &&