divides large triangles for better rendering

git-svn-id: https://hrpsys-base.googlecode.com/svn/trunk@372 a991ac11-fb38-5095-8c12-a1ddb0715245

doc/utilities.h

@@ -19,6 +19,7 @@ hrpsys-simulator [project file] [options]
 -showsensor show sensor output<br>
 -size size specify initial window size<br>
 -no-default-lights turn off default lights<br>
+-max-edge-length divide large triangles which have longer edges than this value<br>
 
 Note:NameSever and openhrp-model-loader must be running
 
@@ -59,6 +60,7 @@ hrpsys-viewer [model file] [-size size]
 
 -size specify initial window size<br>
 -no-default-lights turn off default lights<br>
+-max-edge-length divide large triangles which have longer edges than this value<br>
 
 Note:NameSever and openhrp-model-loader must be running
 

lib/util/GLbody.cpp

@@ -88,3 +88,11 @@ boost::function2<void, hrp::Body *, hrp::Sensor *> GLbody::getSensorDrawCallback
 {
  return m_sensorDrawCallback;
 }
+
+void GLbody::divideLargeTriangles(double maxEdgeLen)
+{
+ for (int i=0; i<numLinks(); i++){
+ GLlink *l = (GLlink *)link(i);
+ l->divideLargeTriangles(maxEdgeLen);
+ }
+}

lib/util/GLbody.h

@@ -29,6 +29,7 @@ class GLbody : virtual public hrp::Body
  GLcamera *findCamera(const char *i_name);
  void setSensorDrawCallback(boost::function2<void, hrp::Body *, hrp::Sensor *> f);
  boost::function2<void, hrp::Body *, hrp::Sensor *> getSensorDrawCallback();
+ void divideLargeTriangles(double maxEdgeLen);
  static void useAbsTransformToDraw();
 
 private:

lib/util/GLlink.cpp

@@ -248,3 +248,10 @@ void GLlink::highlight(bool flag)
  m_cameras[i]->highlight(flag);
  }
 }
+
+void GLlink::divideLargeTriangles(double maxEdgeLen)
+{
+ for (size_t i=0; i<m_shapes.size(); i++){
+ m_shapes[i]->divideLargeTriangles(maxEdgeLen);
+ }
+}

lib/util/GLlink.h

@@ -33,6 +33,7 @@ class GLlink : public hrp::Link, public GLcoordinates
  void addCamera(GLcamera *camera);
  void showAxes(bool flag);
  void highlight(bool flag);
+ void divideLargeTriangles(double maxEdgeLen);
  const std::vector<GLcamera *>& cameras();
  static void useAbsTransformToDraw();
  static int drawMode();

lib/util/GLsceneBase.cpp

@@ -29,7 +29,8 @@ GLsceneBase::GLsceneBase(LogManagerBase *i_log) :
  m_width(DEFAULT_W), m_height(DEFAULT_H),
  m_showingStatus(false), m_showSlider(false),
  m_log(i_log), m_videoWriter(NULL), m_cvImage(NULL), 
- m_showFloorGrid(true), m_showInfo(true), m_request(REQ_NONE)
+ m_showFloorGrid(true), m_showInfo(true), m_request(REQ_NONE), 
+ m_maxEdgeLen(0)
 {
  m_default_camera = new GLcamera(DEFAULT_W, DEFAULT_H, 0.1, 100.0, 30*M_PI/180);
  m_default_camera->setViewPoint(4,0,0.8);
@@ -366,3 +367,17 @@ void GLsceneBase::setView()
  glViewport(0,0,m_width, m_height);
  m_camera->setView(m_width, m_height);
 }
+
+void GLsceneBase::addBody(hrp::BodyPtr i_body)
+{
+ if (m_maxEdgeLen){
+ GLbody *glbody = dynamic_cast<GLbody *>(i_body.get());
+ if (glbody) glbody->divideLargeTriangles(m_maxEdgeLen);
+ }
+ WorldBase::addBody(i_body);
+}
+
+void GLsceneBase::maxEdgeLen(double i_len)
+{
+ m_maxEdgeLen = i_len;
+}

lib/util/GLsceneBase.h

@@ -52,6 +52,8 @@ class GLsceneBase : virtual public hrp::World<hrp::ConstraintForceSolver>
  void showFloorGrid(bool flag);
  bool showFloorGrid();
  void showInfo(bool flag);
+ void addBody(hrp::BodyPtr i_body);
+ void maxEdgeLen(double i_len);
 protected:
  enum {REQ_NONE, REQ_CLEAR, REQ_CAPTURE};
 
@@ -70,6 +72,7 @@ class GLsceneBase : virtual public hrp::World<hrp::ConstraintForceSolver>
  bool m_showFloorGrid, m_showInfo;
  int m_request;
  std::string m_fname;
+ double m_maxEdgeLen;
 };
 
 #endif

lib/util/GLshape.cpp

@@ -1,4 +1,5 @@
 #include <iostream>
+#include <deque>
 #ifdef __APPLE__
 #include <OpenGL/glu.h>
 #else
@@ -251,3 +252,113 @@ void GLshape::highlight(bool flag)
  if (m_highlight != flag) compile();
  m_highlight = flag;
 }
+
+void GLshape::divideLargeTriangles(double maxEdgeLen)
+{
+ std::vector<Eigen::Vector3i> new_triangles;
+ std::vector<int> new_normalIndices, new_textureCoordIndices;
+
+ double scale[3];
+ for (int i=0; i<3; i++){
+ scale[i] = sqrt(m_trans[i]*m_trans[i]
+ +m_trans[i+4]*m_trans[i+4]
+ +m_trans[i+8]*m_trans[i+8]);
+ }
+ //std::cout << "normal per vertex:" << m_normalPerVertex << std::endl;
+ for (size_t i=0; i<m_triangles.size(); i++){
+ std::deque<Eigen::Vector3i> dq_triangles;
+ dq_triangles.push_back(m_triangles[i]);
+ std::deque<Eigen::Vector3i> dq_normals;
+ Eigen::Vector3i n;
+ if (m_normalPerVertex){
+ for (int j=0; j<3; j++){
+ if (m_normalIndices.size() == 0){
+ n[j] = m_triangles[i][j];
+ }else{
+ n[j] = m_normalIndices[i*3+j];
+ }
+ }
+ }else{
+ if (m_normalIndices.size() == 0){
+ n[0] = i;
+ }else{
+ n[0] = m_normalIndices[i];
+ }
+ }
+ dq_normals.push_back(n);
+ std::deque<Eigen::Vector3i> dq_textureCoordIndices;
+ if (m_texture){
+ dq_textureCoordIndices.push_back(
+ Eigen::Vector3i(m_textureCoordIndices[i*3],
+ m_textureCoordIndices[i*3+1],
+ m_textureCoordIndices[i*3+2]));
+ }
+ while(dq_triangles.size()){
+ Eigen::Vector3i tri = dq_triangles.front();
+ dq_triangles.pop_front();
+ Eigen::Vector3i n = dq_normals.front();
+ dq_normals.pop_front();
+ Eigen::Vector3i tc;
+ if (m_texture){
+ tc = dq_textureCoordIndices.front();
+ dq_textureCoordIndices.pop_front();
+ }
+ //
+ double l[3];
+ for (int j=0; j<3; j++){
+ Eigen::Vector3f e = m_vertices[tri[j]] - m_vertices[tri[(j+1)%3]];
+ for (int k=0; k<3; k++) e[k] *= scale[k];
+ l[j] = e.norm();
+ }
+ int maxidx;
+ if (l[0] > l[1]){
+ maxidx = l[0] > l[2] ? 0 : 2;
+ }else{
+ maxidx = l[1] > l[2] ? 1 : 2;
+ }
+ if (l[maxidx] <= maxEdgeLen){
+ new_triangles.push_back(tri);
+ if (m_normalPerVertex){
+ for (int j=0; j<3; j++) new_normalIndices.push_back(n[j]);
+ }else{
+ new_normalIndices.push_back(n[0]);
+ }
+ for (int j=0; j<3; j++) new_textureCoordIndices.push_back(tc[j]);
+ }else{
+ int vnew = m_vertices.size();
+ m_vertices.push_back(
+ (m_vertices[tri[maxidx]]+m_vertices[tri[(maxidx+1)%3]])/2);
+ dq_triangles.push_back(
+ Eigen::Vector3i(tri[maxidx], vnew, tri[(maxidx+2)%3]));
+ dq_triangles.push_back(
+ Eigen::Vector3i(vnew,tri[(maxidx+1)%3],tri[(maxidx+2)%3]));
+ if (m_normalPerVertex){
+ int nnew = m_normals.size();
+ m_normals.push_back(
+ (m_normals[n[maxidx]]+m_normals[n[(maxidx+1)%3]])/2);
+ dq_normals.push_back(
+ Eigen::Vector3i(n[maxidx], nnew, n[(maxidx+2)%3]));
+ dq_normals.push_back(
+ Eigen::Vector3i(nnew,n[(maxidx+1)%3],n[(maxidx+2)%3]));
+ }else{
+ dq_normals.push_back(n);
+ dq_normals.push_back(n);
+ }
+ if (m_texture){
+ int tcnew = m_textureCoordinates.size();
+ m_textureCoordinates.push_back(
+ (m_textureCoordinates[tc[maxidx]]
+ +m_textureCoordinates[tc[(maxidx+1)%3]])/2);
+ dq_textureCoordIndices.push_back(
+ Eigen::Vector3i(tc[maxidx], tcnew, tc[(maxidx+2)%3]));
+ dq_textureCoordIndices.push_back(
+ Eigen::Vector3i(tcnew, tc[(maxidx+1)%3], tc[(maxidx+2)%3]));
+ }
+ }
+ }
+ }
+
+ m_triangles = new_triangles;
+ m_normalIndices = new_normalIndices;
+ m_textureCoordIndices = new_textureCoordIndices;
+}

lib/util/GLshape.h

@@ -34,13 +34,14 @@ class GLshape : public GLcoordinates
  void setTexture(GLtexture *texture);
  void compile();
  void highlight(bool flag);
+ void divideLargeTriangles(double maxEdgeLen);
 protected:
  int doCompile(bool isWireFrameMode);
 
  std::vector<Eigen::Vector3f> m_vertices, m_normals;
  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > m_textureCoordinates;
  std::vector<Eigen::Vector3i> m_triangles;
- Eigen::VectorXi m_normalIndices, m_textureCoordIndices;
+ std::vector<int> m_normalIndices, m_textureCoordIndices;
  float m_diffuse[4], m_specular[4], m_shininess;
  bool m_normalPerVertex;
  bool m_solid;

util/simulator/main.cpp

@@ -74,6 +74,7 @@ int main(int argc, char* argv[])
  bool showsensors = false;
  int wsize = 0;
  bool useDefaultLights = true;
+ double maxEdgeLen = 0;
  for (int i=0; i<argc; i++){
  if (strcmp("-nodisplay",argv[i])==0){
  display = false;
@@ -89,6 +90,8 @@ int main(int argc, char* argv[])
  wsize = atoi(argv[++i]);
  }else if(strcmp("-no-default-lights", argv[i])==0){
  useDefaultLights = false;
+ }else if(strcmp("-max-edge-length", argv[i])==0){
+ maxEdgeLen = atof(argv[++i]);
  }
  }
 
@@ -110,6 +113,7 @@ int main(int argc, char* argv[])
  && strcmp(argv[i], "-showsensors")
  && strcmp(argv[i], "-size")
  && strcmp(argv[i], "-no-default-lights")
+ && strcmp(argv[i], "-max-edge-length")
  ){
  rtmargv.push_back(argv[i]);
  rtmargc++;
@@ -134,6 +138,7 @@ int main(int argc, char* argv[])
  LogManager<SceneState> log;
  GLscene scene(&log);
  scene.showSensors(showsensors);
+ scene.maxEdgeLen(maxEdgeLen);
  Simulator simulator(&log);
 
  SDLwindow window(&scene, &log, &simulator);

util/viewer/main.cpp

@@ -60,6 +60,17 @@ int main(int argc, char *argv[])
 
  poaManager->activate();
 
+ int wsize=0;
+ bool useDefaultLights=true;
+ for (int i=1; i<argc; i++){
+ if (strcmp(argv[i], "-size")==0){
+ wsize = atoi(argv[++i]);
+ }else if(strcmp(argv[i], "-max-edge-length")==0){
+ scene.maxEdgeLen(atof(argv[++i]));
+ }else if(strcmp(argv[i], "-no-default-lights")==0){
+ useDefaultLights=false;
+ }
+ }
  if (argc >= 2 && argv[1][0] != '-'){
  OpenHRP::ModelLoader_var ml = hrp::getModelLoader(namingContext);
  OpenHRP::ModelLoader::ModelLoadOption opt;
@@ -78,16 +89,7 @@ int main(int argc, char *argv[])
  OpenHRP::BodyInfo_var binfo = ml->getBodyInfoEx(url.c_str(), opt);
  hrp::loadBodyFromBodyInfo(body, binfo, false, GLlinkFactory);
  loadShapeFromBodyInfo(glbody, binfo);
- scene.WorldBase::addBody(body);
- }
- int wsize=0;
- bool useDefaultLights=true;
- for (int i=1; i<argc; i++){
- if (strcmp(argv[i], "-size")==0){
- wsize = atoi(argv[++i]);
- }else if(strcmp(argv[i], "-no-default-lights")==0){
- useDefaultLights=false;
- }
+ scene.addBody(body);
  }
 
  GLlink::useAbsTransformToDraw();