Logo Search packages:      
Sourcecode: vdrift version File versions

bezier.cpp

/***************************************************************************
 *            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) && QUAD_DIV_FAST_DISCARD)
                  //if (QUAD_DIV_FAST_DISCARD)
                  {
                        outtri = origin;
                        return false;
                  }
                  else
                  {
                        if (verbose)
                        {
                              cout << "<" << i << ": nocol " << su << "," << sv << ">" << endl;
                              cout << "<" << umin << "," << umax << ";" << vmin << "," << vmax << ">" << endl;
                              
                              ul.DebugPrint();
                              ur.DebugPrint();
                              bl.DebugPrint();
                              br.DebugPrint();
                              
                              cout << endl;
                        }
                        
                        loop = false;
                  }
                  
                  /*if (i != 0)
                  {
                        loop = false;
                        u = oldu;
                        v = oldv;
                  }*/
            }
      }
      
      if (col)// || QUAD_DIV_FAST_DISCARD)
      //if (col)
      {
            /*if (!col)
                  cout << "blip " << su << "," << sv << ": " << u << "," << v << endl;*/
            outtri = SurfCoord(su, sv);
            normal = SurfNorm(su, sv);
            //if (verbose)
                  //cout << "<" << i << ">" << endl;
            return true;
      }
      else
      {
            outtri = origin;
            return false;
      }
}

Generated by  Doxygen 1.6.0   Back to index