Fix elbow rotation.

README.md

@@ -57,7 +57,7 @@ When I am implementing the B-Mesh algorithm, I am also think in the future, how
 After finish the rotation at the two connected bones, I need implement 3D Convex Hull algorithm at the joint ball, there are so many methods to get the convex hull, I found the [Gift wrapping](http://dccg.upc.edu/people/vera/wp-content/uploads/2014/11/GA2014-ConvexHulls3D-Roger-Hernando.pdf) is the most strait-forward one, though is not the most efficient one.  
 <img src="screenshot/dust3d_convex_hull.png" width="124" height="128">  
 *Stitching*  
-I follow the B-Mesh paper, made another test module inside Blender, and created a correspond `data/bmesh_test_2.h` manually.
+I follow the B-Mesh paper, made another test module inside Blender, and created a correspond `data/bmesh_test_2.h` manually.  
 <img src="screenshot/dust3d_bmesh_test_2.png" width="124" height="128">  
 - [ ] Export Wavefront .obj  
 - [ ] Render B-Mesh result  

data/bmesh_test_2.h

@@ -5,7 +5,7 @@
 #define THIRD_BALL_SIZE   0.4
 
 const float bmeshTestBalls[][6] = {
-  /*Head*/        {0,  0.00000,  5.39143,  0.61970, BIG_BALL_SIZE},
+  /*Head*/        {0,  0.00000,  5.39143,  0.61970, BIG_BALL_SIZE, 1},
   /*Neck*/        {1,  0.00000,  4.39990, -0.21690, THIRD_BALL_SIZE},
   /*Under Neck*/  {2,  0.00000,  2.88163,  0.00000, THIRD_BALL_SIZE, 1},
   /*Chest*/       {3,  0.00000,  1.39434,  0.61970, SECOND_BALL_SIZE},
@@ -18,7 +18,7 @@ const float bmeshTestBalls[][6] = {
   /*LShoulder*/   {10,+1.42532,  2.72670, -0.44708, THIRD_BALL_SIZE},
   /*LElbow1*/     {11,+3.12951,  1.95207, -1.07872, THIRD_BALL_SIZE},
   /*LElbow2*/     {12,+2.91261,  1.08448, -0.68593, THIRD_BALL_SIZE},
-  /*LPalm1*/      {13,+1.76616,  0.53399,  0.09306, TINY_BALL_SIZE},
+  /*LPalm1*/      {13,+1.76616,  0.53399,  0.09306, TINY_BALL_SIZE, 1},
   /*LPalm2*/      {14,+1.27039,  0.49576,  0.53458, TINY_BALL_SIZE},
   /*LPalm3*/      {15,+1.48729,  0.03099,  0.34054, TINY_BALL_SIZE},
   /*RKnee*/       {16,-1.82813, -1.64222,  0.30091, THIRD_BALL_SIZE}, 
@@ -27,7 +27,7 @@ const float bmeshTestBalls[][6] = {
   /*RShoulder*/   {19,-1.42532,  2.72670, -0.44708, THIRD_BALL_SIZE},
   /*RElbow1*/     {20,-3.12951,  1.95207, -1.07872, THIRD_BALL_SIZE},
   /*RElbow2*/     {21,-2.91261,  1.08448, -0.68593, THIRD_BALL_SIZE},
-  /*RPalm1*/      {22,-1.76616,  0.53399,  0.09306, TINY_BALL_SIZE},
+  /*RPalm1*/      {22,-1.76616,  0.53399,  0.09306, TINY_BALL_SIZE, 1},
   /*RPalm2*/      {23,-1.27039,  0.49576,  0.53458, TINY_BALL_SIZE},
   /*RPalm3*/      {24,-1.48729,  0.03099,  0.34054, TINY_BALL_SIZE},
   /*Mouth*/       {25, 0.00000,  4.57765,  1.92032, TINY_BALL_SIZE},

src/bmesh.c

@@ -229,8 +229,6 @@ static int bmeshGenerateInbetweenBallsBetween(bmesh *bm,
   vec3 localYaxis;
   vec3 boneDirection;
   vec3 normalizedBoneDirection;
-  vec3 worldYaxis = {0, 1, 0};
-  vec3 worldXaxis = {1, 0, 0};
   bmeshBall *firstBall = bmeshGetBall(bm, firstBallIndex);
   bmeshBall *secondBall = bmeshGetBall(bm, secondBallIndex);
   bmeshBall *newBall;
@@ -268,7 +266,7 @@ static int bmeshGenerateInbetweenBallsBetween(bmesh *bm,
     step += remaining / calculatedStepCount;
     offset = step;
     if (offset < distance) {
-      while (offset < distance) {
+      while (offset < distance && offset + BMESH_STEP_DISTANCE <= distance) {
         float frac = offset / distance;
         parentBallIndex = bmeshAddInbetweenBallBetween(bm,
           firstBall, secondBall, frac, parentBallIndex);
@@ -402,9 +400,12 @@ int bmeshAddQuad(bmesh *bm, quad *q) {
 
 static int bmeshSweepFrom(bmesh *bm, bmeshBall *parent, bmeshBall *ball) {
   int result = 0;
-  vec3 worldYaxis = {0, 1, 0};
   bmeshBallIterator iterator;
   bmeshBall *child = 0;
+  if (bm->roundColor == ball->roundColor) {
+    return 0;
+  }
+  ball->roundColor = bm->roundColor;
   if (BMESH_BALL_TYPE_KEY == ball->type) {
     child = bmeshGetBallFirstChild(bm, ball, &iterator);
     if (child) {
@@ -414,41 +415,55 @@ static int bmeshSweepFrom(bmesh *bm, bmeshBall *parent, bmeshBall *ball) {
         vec3CrossProduct(&parent->boneDirection, &child->boneDirection,
           &rotateAxis);
         vec3Normalize(&rotateAxis);
-        vec3Add(&rotateAxis, &ball->position, &rotateAxis);
         rotateAngle = vec3Angle(&parent->boneDirection, &child->boneDirection);
+        /*
+        if (1 || 11 == ball->index || 20 == ball->index) {
+          glColor3f(0.0, 0.0, 0.0);
+          drawDebugPrintf("<%f,%f,%f> <%f,%f,%f> <%f,%f,%f> rotateAngle:%f",
+            parent->boneDirection.x,
+            parent->boneDirection.y,
+            parent->boneDirection.z,
+            child->boneDirection.x,
+            child->boneDirection.y,
+            child->boneDirection.z,
+            rotateAxis.x,
+            rotateAxis.y,
+            rotateAxis.z,
+            rotateAngle);
+        }*/
         rotateAngle *= 0.5;
         
         /*
-        glColor3f(0.0, 0.0, 0.0);
+        if (11 == ball->index) {
-        drawDebugPrintf("<%f,%f,%f> <%f,%f,%f> rotateAngle:%f",
+          glPushMatrix();
-          parent->boneDirection.x,
+          //glTranslatef(parent->position.x, parent->position.y, parent->position.z);
-          parent->boneDirection.y,
+          glColor3f(1.0, 0.0, 1.0);
-          parent->boneDirection.z,
+          glBegin(GL_LINES);
-          child->boneDirection.x,
+          glVertex3f(0, 0, 0);
-          child->boneDirection.y,
+          glVertex3f(parent->boneDirection.x, parent->boneDirection.y,
-          child->boneDirection.z,
+            parent->boneDirection.z);
-          rotateAngle);
+          glEnd();
-        
+          glPopMatrix();
-        glPushMatrix();
+          
-        glTranslatef(parent->position.x, parent->position.y, parent->position.z);
+          glPushMatrix();
-        glColor3f(1.0, 0.0, 1.0);
+          //glTranslatef(child->position.x, child->position.y, child->position.z);
-        glBegin(GL_LINES);
+          glColor3f(0.0, 1.0, 1.0);
-        glVertex3f(0, 0, 0);
+          glBegin(GL_LINES);
-        glVertex3f(parent->boneDirection.x, parent->boneDirection.y,
+          glVertex3f(0, 0, 0);
-          parent->boneDirection.z);
+          glVertex3f(child->boneDirection.x, child->boneDirection.y,
-        glEnd();
+            child->boneDirection.z);
-        glPopMatrix();
+          glEnd();
-        
+          glPopMatrix();
-        glPushMatrix();
+          
-        glTranslatef(child->position.x, child->position.y, child->position.z);
+          glPushMatrix();
-        glColor3f(0.0, 1.0, 1.0);
+          //glTranslatef(ball->position.x, ball->position.y, ball->position.z);
-        glBegin(GL_LINES);
+          glColor3f(1.0, 0.0, 0.0);
-        glVertex3f(0, 0, 0);
+          glBegin(GL_LINES);
-        glVertex3f(child->boneDirection.x, child->boneDirection.y,
+          glVertex3f(0, 0, 0);
-          child->boneDirection.z);
+          glVertex3f(rotateAxis.x, rotateAxis.y, rotateAxis.z);
-        glEnd();
+          glEnd();
-        glPopMatrix();
+          glPopMatrix();
-        */
+        }*/
         
         ball->boneDirection = parent->boneDirection;
         vec3RotateAlong(&ball->boneDirection, rotateAngle, &rotateAxis,
@@ -460,11 +475,14 @@ static int bmeshSweepFrom(bmesh *bm, bmeshBall *parent, bmeshBall *ball) {
       }
     } else {
       ball->boneDirection = parent->boneDirection;
+      generateYZfromBoneDirection(&ball->boneDirection,
+        &ball->localYaxis, &ball->localZaxis);
+      /*
       vec3CrossProduct(&worldYaxis, &ball->boneDirection, &ball->localYaxis);
       vec3Normalize(&ball->localYaxis);
       vec3CrossProduct(&ball->localYaxis, &ball->boneDirection,
         &ball->localZaxis);
-      vec3Normalize(&ball->localZaxis);
+      vec3Normalize(&ball->localZaxis);*/
     }
   }
   for (child = bmeshGetBallFirstChild(bm, ball, &iterator);
@@ -480,6 +498,7 @@ static int bmeshSweepFrom(bmesh *bm, bmeshBall *parent, bmeshBall *ball) {
 }
 
 int bmeshSweep(bmesh *bm) {
+  bm->roundColor++;
   return bmeshSweepFrom(bm, 0, bmeshGetRootBall(bm));
 }
 
@@ -488,7 +507,7 @@ static bmeshBall *bmeshFindBallForConvexHull(bmesh *bm, bmeshBall *root,
   bmeshBallIterator iterator;
   bmeshBall *child;
   float distance = vec3Distance(&root->position, &ball->position);
-  if (distance > root->radius) {
+  if (distance >= root->radius) {
     return ball;
   }
   child = bmeshGetBallFirstChild(bm, ball, &iterator);
@@ -499,47 +518,59 @@ static bmeshBall *bmeshFindBallForConvexHull(bmesh *bm, bmeshBall *root,
   return bmeshFindBallForConvexHull(bm, root, child);
 }
 
-static int bmeshStichFrom(bmesh *bm, bmeshBall *ball) {
+static void addBallToHull(convexHull *hull, bmeshBall *ballForConvexHull) {
+  vec3 z, y;
+  quad q;
+  
+  vec3Scale(&ballForConvexHull->localYaxis, ballForConvexHull->radius, &y);
+  vec3Scale(&ballForConvexHull->localZaxis, ballForConvexHull->radius, &z);
+  vec3Sub(&ballForConvexHull->position, &y, &q.pt[0]);
+  vec3Add(&q.pt[0], &z, &q.pt[0]);
+  vec3Sub(&ballForConvexHull->position, &y, &q.pt[1]);
+  vec3Sub(&q.pt[1], &z, &q.pt[1]);
+  vec3Add(&ballForConvexHull->position, &y, &q.pt[2]);
+  vec3Sub(&q.pt[2], &z, &q.pt[2]);
+  vec3Add(&ballForConvexHull->position, &y, &q.pt[3]);
+  vec3Add(&q.pt[3], &z, &q.pt[3]);
+  
+  convexHullAddVertex(hull, &q.pt[0]);
+  convexHullAddVertex(hull, &q.pt[1]);
+  convexHullAddVertex(hull, &q.pt[2]);
+  convexHullAddVertex(hull, &q.pt[3]);
+}
+
+static int bmeshStichFrom(bmesh *bm, bmeshBall *parent, bmeshBall *ball) {
   int result = 0;
   bmeshBallIterator iterator;
   bmeshBall *child;
   bmeshBall *ballForConvexHull;
+  if (bm->roundColor == ball->roundColor) {
+    return 0;
+  }
+  ball->roundColor = bm->roundColor;
   if (BMESH_BALL_TYPE_ROOT == ball->type) {
     convexHull *hull = convexHullCreate();
     if (!hull) {
       fprintf(stderr, "%s:convexHullCreate failed.\n", __FUNCTION__);
       return -1;
     }
+    glColor3f(0.0, 0.0, 0.0);
+    drawDebugPrintf("root <%f,%f,%f>", ball->position.x,
+      ball->position.y, ball->position.z);
     for (child = bmeshGetBallFirstChild(bm, ball, &iterator);
         child;
         child = bmeshGetBallNextChild(bm, ball, &iterator)) {
-      vec3 z, y;
-      quad q;
-      int vertexIndices[4];
-      
       ballForConvexHull = bmeshFindBallForConvexHull(bm, ball, child);
-      
+      addBallToHull(hull, ballForConvexHull);
-      vec3Scale(&ballForConvexHull->localYaxis, ballForConvexHull->radius, &y);
+    }
-      vec3Scale(&ballForConvexHull->localZaxis, ballForConvexHull->radius, &z);
+    if (parent) {
-      vec3Sub(&ballForConvexHull->position, &y, &q.pt[0]);
+      addBallToHull(hull, parent);
-      vec3Add(&q.pt[0], &z, &q.pt[0]);
-      vec3Sub(&ballForConvexHull->position, &y, &q.pt[1]);
-      vec3Sub(&q.pt[1], &z, &q.pt[1]);
-      vec3Add(&ballForConvexHull->position, &y, &q.pt[2]);
-      vec3Sub(&q.pt[2], &z, &q.pt[2]);
-      vec3Add(&ballForConvexHull->position, &y, &q.pt[3]);
-      vec3Add(&q.pt[3], &z, &q.pt[3]);
-      
-      vertexIndices[0] = convexHullAddVertex(hull, &q.pt[0]);
-      vertexIndices[1] = convexHullAddVertex(hull, &q.pt[1]);
-      vertexIndices[2] = convexHullAddVertex(hull, &q.pt[2]);
-      vertexIndices[3] = convexHullAddVertex(hull, &q.pt[3]);
     }
     convexHullGenerate(hull);
     glPushMatrix();
     {
       int triIndex;
-      glColor4f(1.0f, 1.0f, 1.0f, 0.5);
+      glColor3f(1.0f, 1.0f, 1.0f);
       for (triIndex = 0; triIndex < convexHullGetTriangleNum(hull);
           ++triIndex) {
         triangle *tri = (triangle *)convexHullGetTriangle(hull, triIndex);
@@ -568,7 +599,7 @@ static int bmeshStichFrom(bmesh *bm, bmeshBall *ball) {
   for (child = bmeshGetBallFirstChild(bm, ball, &iterator);
       child;
       child = bmeshGetBallNextChild(bm, ball, &iterator)) {
-    result = bmeshSweepFrom(bm, ball, child);
+    result = bmeshStichFrom(bm, ball, child);
     if (0 != result) {
       fprintf(stderr, "%s:bmeshSweepFrom failed.\n", __FUNCTION__);
       return result;
@@ -578,5 +609,6 @@ static int bmeshStichFrom(bmesh *bm, bmeshBall *ball) {
 }
 
 int bmeshStitch(bmesh *bm) {
-  return bmeshStichFrom(bm, bmeshGetRootBall(bm));
+  bm->roundColor++;
+  return bmeshStichFrom(bm, 0, bmeshGetRootBall(bm));
 }

src/render.cpp

@@ -274,10 +274,12 @@ void Render::paintGL() {
       bmeshBall *ball = bmeshGetBall(bm, index);
       drawBmeshBall(bm, ball);
     }*/
+    
     for (index = 0; index < bmeshGetBoneNum(bm); ++index) {
       bmeshBone *bone = bmeshGetBone(bm, index);
       drawBmeshBone(bm, bone);
     }
+    /*
     glColor4f(1.0f, 1.0f, 1.0f, 0.5);
     glBegin(GL_QUADS);
     for (index = 0; index < bmeshGetQuadNum(bm); ++index) {
@@ -301,7 +303,7 @@ void Render::paintGL() {
       }
       glVertex3f(q->pt[0].x, q->pt[0].y, q->pt[0].z);
       glEnd();
-    }
+    }*/
   }
 
   glPopMatrix();