roadgeom.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 630 行 · 第 1/2 页

      // Figure our where we are in the profile
      ti = (int) (floor(t / this->profileScale));
      tm = t / this->profileScale - ti;
      ti += point->profilePointCount / 2;
      //printf("t ti tm %f %d %f\n", t, ti, tm);      

      // Spline control point indexes
      n0 = ti - 1;
      n1 = ti + 0;
      n2 = ti + 1;
      n3 = ti + 2;
      
      // Handle boundary conditions for spline fit
      n0 = Min(Max(0, n0), point->profilePointCount - 1);
      n1 = Min(Max(0, n1), point->profilePointCount - 1);
      n2 = Min(Max(0, n2), point->profilePointCount - 1);
      n3 = Min(Max(0, n3), point->profilePointCount - 1);

      assert(n0 >= 0 && n0 < point->profilePointCount);
      assert(n1 >= 0 && n1 < point->profilePointCount);
      assert(n2 >= 0 && n2 < point->profilePointCount);
      assert(n3 >= 0 && n3 < point->profilePointCount);
        
      // Use a spline to interpolate our altitude
      p = this->SplineCR(tm,
                         point->profilePoints[n0],
                         point->profilePoints[n1],
                         point->profilePoints[n2],
                         point->profilePoints[n3]);
      
      // Update the vertex position
      vertex[0] = o.x - ot.y * t;
      vertex[1] = o.y + ot.x * t;      
      vertex[2] = o.z + p.z;
    }
  }

  // Compute normal vectors
  this->UpdateNormals();

  // Construct the trimesh data
  dGeomTriMeshDataBuildSimple(this->tridata,
                              (dReal*) this->vertices, this->vertexCount,
                              this->indices, this->indexCount);
  dGeomTriMeshSetData(this->trimesh, this->tridata);

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Compute normal vectors on vertices
void RoadGeom::UpdateNormals()
{
  int i, a, b, c;
  dVector3 *v;
  GzVector v0, v1, v2, n;

  v = this->vertices;

  // Reset the normals
  for (i = 0; i < this->vertexCount; i++)
    this->normals[i] = GzVectorZero();

  // Consider each triangle
  for (i = 0; i < this->indexCount; i += 3)
  {
    a = this->indices[i + 0];
    b = this->indices[i + 1];
    c = this->indices[i + 2];

    // Compute the normal
    v0 = GzVectorSet(v[a][0], v[a][1], v[a][2]);
    v1 = GzVectorSet(v[b][0], v[b][1], v[b][2]);
    v2 = GzVectorSet(v[c][0], v[c][1], v[c][2]);
    n = GzVectorCross(GzVectorSub(v1, v0), GzVectorSub(v2, v1));
    n = GzVectorUnit(n);

    // Accumate normals across triangles
    // We assume normals are being normalized by OpenGL
    this->normals[a] = GzVectorAdd(this->normals[a], n);
    this->normals[b] = GzVectorAdd(this->normals[b], n);
    this->normals[c] = GzVectorAdd(this->normals[c], n);
  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Compute a Catmull-Rom spline
GzVector RoadGeom::SplineCR(double t, GzVector p0, GzVector p1,
                            GzVector p2, GzVector p3)
{
  GzVector q;

  q.x = 2*p1.x +
    (-p0.x + p2.x) * t +
    (2*p0.x - 5 * p1.x + 4*p2.x - p3.x) * t*t +
    (-p0.x + 3*p1.x - 3*p2.x + p3.x) * t*t*t;

  q.y = 2*p1.y +
    (-p0.y + p2.y) * t +
    (2*p0.y - 5 * p1.y + 4*p2.y - p3.y) * t*t +
    (-p0.y + 3*p1.y - 3*p2.y + p3.y) * t*t*t;

  q.z = 2*p1.z +
    (-p0.z + p2.z) * t +
    (2*p0.z - 5 * p1.z + 4*p2.z - p3.z) * t*t +
    (-p0.z + 3*p1.z - 3*p2.z + p3.z) * t*t*t;

  q = GzVectorMul(0.5, q);
  
  return q;
}


//////////////////////////////////////////////////////////////////////////////
// Compute a Catmull-Rom spline tangent
GzVector RoadGeom::SplineCRtan(double t, GzVector p0, GzVector p1,
                               GzVector p2, GzVector p3)
{
  GzVector v0, v1, v;
  
  v0 = GzVectorSub(p2, p0);
  v0 = GzVectorMul(0.5, v0);
  v1 = GzVectorSub(p3, p1);
  v1 = GzVectorMul(0.5, v1);

  v = GzVectorMul((1 - t), v0);
  v = GzVectorAdd(v, GzVectorMul(t, v1));

  v = GzVectorUnit(v);
  
  return v;
}


//////////////////////////////////////////////////////////////////////////////
// Default render routine
void RoadGeom::Render(RenderOptions *opt)
{
  int i, j;
  int ni, nj, mi, mj;
  GLuint listId;
  RenderOptions listOpt;
  
  // Recover stored display list for this camera
  this->GetList(opt->cameraIndex, &listId, &listOpt);

  // See if the current display list is dirty
  this->dirty |= (listId == 0);
  this->dirty |= (opt->displayMaterials != listOpt.displayMaterials);
  this->dirty |= (opt->displayTextures != listOpt.displayTextures);

  // Compute offset based on old camera pose
  mi = (int) floor(listOpt.cameraPose.pos.x / this->patchSize + 0.5);
  mj = (int) floor(listOpt.cameraPose.pos.y / this->patchSize + 0.5);

  // Compute offset based on new camera pose
  ni = (int) floor(opt->cameraPose.pos.x / this->patchSize + 0.5);
  nj = (int) floor(opt->cameraPose.pos.y / this->patchSize + 0.5);

  // If the offset has changed, we need to redraw
  this->dirty |= (ni != mi);
  this->dirty |= (nj != mj);

  // Generate the display list
  if (this->dirty)
  {
    if (listId == 0)
      listId = glGenLists(1);    
    glNewList(listId, GL_COMPILE);

    // Set material properties
    if (opt->displayMaterials)
    {
      glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, this->colorAmbient);
      glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, this->colorDiffuse);
      glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, this->colorSpecular);
      glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, this->shininess);
    }

    if (opt->displayTextures && this->textureImage)
    {
      // Set up textures
      glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
      glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
      glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
      glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
      glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);

      // Build the mipmaps
      // Assume image data is *not* aligned
      glPixelStorei( GL_UNPACK_ALIGNMENT, 1);
      gluBuild2DMipmaps(GL_TEXTURE_2D, this->textureImage->components,
                        this->textureImage->width, this->textureImage->height,
                        this->textureImage->format, this->textureImage->type,
                        this->textureImage->data);
      
      glEnable(GL_TEXTURE_2D);
    }

    dReal *v;
    GzVector n;
    double tx, ty;

    // Texture scale; each texture image is scaled such that it maps
    // to a road patch of size textureSize
    tx = this->scale / this->textureSize.x;
    ty = this->scale / this->textureSize.y;
    
    // Construct triangle strips
    for (i = 0; i < this->stepsX - 1; i++)
    {
      // Test vertex to see if it is near the current camera position
      v = this->vertices[this->stepsY * i + this->stepsY / 2];
      if (fabs(v[0] - opt->cameraPose.pos.x) > opt->farClip)
        continue;
      if (fabs(v[1] - opt->cameraPose.pos.y) > opt->farClip)
        continue;

      glBegin(GL_TRIANGLE_STRIP);
      
      for (j = 0; j < this->stepsY; j++)
      {
        glTexCoord2f(i * tx, j * tx);        
        n = this->normals[this->stepsY * (i + 0) + j];
        v = this->vertices[this->stepsY * (i + 0) + j];
        glNormal3f(n.x, n.y, n.z);
        glVertex3f(v[0], v[1], v[2]);

        glTexCoord2f((i + 1) * tx, j * tx);
        n = this->normals[this->stepsY * (i + 1) + j];
        v = this->vertices[this->stepsY * (i + 1) + j];        
        glNormal3f(n.x, n.y, n.z);
        glVertex3f(v[0], v[1], v[2]);
      }

      glEnd();
    }

    if (opt->displayTextures && this->textureImage)
      glDisable(GL_TEXTURE_2D);

    // Display signs
    this->RenderSigns(opt, listOpt);
    
    // Store list options
    this->SetList(opt->cameraIndex, listId, *opt);      
    glEndList();
  }
  
  // Call the display list
  if (listId)
    glCallList(listId);
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Render signs
void RoadGeom::RenderSigns(RenderOptions *opt, RenderOptions listOpt)
{
  int i, j;
  double size;
  GzVector p;
  SignData *sign;

  // Set material properties
  if (opt->displayMaterials)
  {
    glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, GzColor(0, 0, 0));
    glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, GzColor(0, 0, 0));
    glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, GzColor(0, 0, 0));
  }

  for (i = 0; i < this->signCount; i++)
  {
    sign = this->signs + i;
    p = sign->pos;
        
    // Test sign to see if it is near the current camera position
    if (fabs(p.x - opt->cameraPose.pos.x) > opt->farClip)
      continue;
    if (fabs(p.y - opt->cameraPose.pos.y) > opt->farClip)
      continue;

    // Set sign hight
    size = 0.80;
    
    // Print the sign
    glPushMatrix();
    glTranslatef(p.x, p.y, p.z + 1.0); // HACK
    glRotatef(90, 1, 0, 0);
    glScalef(size / 150, size / 150, size / 150);
    for (j = 0; j < (int) strlen(sign->text); j++)
      glutStrokeCharacter(GLUT_STROKE_MONO_ROMAN, sign->text[j]);
    glPopMatrix();

    // Print a little stand
    glBegin(GL_LINES);
    glVertex3f(p.x, p.y, p.z - size / 2);
    glVertex3f(p.x, p.y, 0.0);
    glEnd();
  }

  return;
}

#endif