Fix elbow rotation.

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,23 +415,28 @@ 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);
-        rotateAngle *= 0.5;
-        
         /*
+        if (1 || 11 == ball->index || 20 == ball->index) {
           glColor3f(0.0, 0.0, 0.0);
-        drawDebugPrintf("<%f,%f,%f> <%f,%f,%f> rotateAngle:%f",
+          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;
         
+        /*
+        if (11 == ball->index) {
           glPushMatrix();
-        glTranslatef(parent->position.x, parent->position.y, parent->position.z);
+          //glTranslatef(parent->position.x, parent->position.y, parent->position.z);
           glColor3f(1.0, 0.0, 1.0);
           glBegin(GL_LINES);
           glVertex3f(0, 0, 0);
@@ -440,7 +446,7 @@ static int bmeshSweepFrom(bmesh *bm, bmeshBall *parent, bmeshBall *ball) {
           glPopMatrix();
           
           glPushMatrix();
-        glTranslatef(child->position.x, child->position.y, child->position.z);
+          //glTranslatef(child->position.x, child->position.y, child->position.z);
           glColor3f(0.0, 1.0, 1.0);
           glBegin(GL_LINES);
           glVertex3f(0, 0, 0);
@@ -448,7 +454,16 @@ static int bmeshSweepFrom(bmesh *bm, bmeshBall *parent, bmeshBall *ball) {
             child->boneDirection.z);
           glEnd();
           glPopMatrix();
-        */
+          
+          glPushMatrix();
+          //glTranslatef(ball->position.x, ball->position.y, ball->position.z);
+          glColor3f(1.0, 0.0, 0.0);
+          glBegin(GL_LINES);
+          glVertex3f(0, 0, 0);
+          glVertex3f(rotateAxis.x, rotateAxis.y, rotateAxis.z);
+          glEnd();
+          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,25 +518,9 @@ 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) {
-  int result = 0;
-  bmeshBallIterator iterator;
-  bmeshBall *child;
-  bmeshBall *ballForConvexHull;
-  if (BMESH_BALL_TYPE_ROOT == ball->type) {
-    convexHull *hull = convexHullCreate();
-    if (!hull) {
-      fprintf(stderr, "%s:convexHullCreate failed.\n", __FUNCTION__);
-      return -1;
-    }
-    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);
   
   vec3Scale(&ballForConvexHull->localYaxis, ballForConvexHull->radius, &y);
   vec3Scale(&ballForConvexHull->localZaxis, ballForConvexHull->radius, &z);
@@ -530,16 +533,44 @@ static int bmeshStichFrom(bmesh *bm, bmeshBall *ball) {
   vec3Add(&ballForConvexHull->position, &y, &q.pt[3]);
   vec3Add(&q.pt[3], &z, &q.pt[3]);
   
-      vertexIndices[0] = convexHullAddVertex(hull, &q.pt[0]);
+  convexHullAddVertex(hull, &q.pt[0]);
-      vertexIndices[1] = convexHullAddVertex(hull, &q.pt[1]);
+  convexHullAddVertex(hull, &q.pt[1]);
-      vertexIndices[2] = convexHullAddVertex(hull, &q.pt[2]);
+  convexHullAddVertex(hull, &q.pt[2]);
-      vertexIndices[3] = convexHullAddVertex(hull, &q.pt[3]);
+  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)) {
+      ballForConvexHull = bmeshFindBallForConvexHull(bm, ball, child);
+      addBallToHull(hull, ballForConvexHull);
+    }
+    if (parent) {
+      addBallToHull(hull, parent);
     }
     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();