diff --git a/README.md b/README.md
index 1ce187cd..2890f0d7 100644
--- a/README.md
+++ b/README.md
@@ -36,7 +36,7 @@ Two caps and many strips composites a cylinder.
 Almost all 3D editor have a infinite grid ground, I just made a finite one, in the future, I should expand the grid outside of the screen to make it infinite.  
 Now, for just beginning, I think it's a not bad start.
 <img src="screenshot/dust3d_sphere_cylinder.png">
-- [ ] Implement B-Mesh algorithm (Dec 18, 2016 ~ Dec 23, 2016)   
+- [ ] Implement B-Mesh algorithm (Dec 18, 2016 ~ Dec 25, 2016)   
 *Drawing Skeletal Shape Balls*  
 Draw shape ball is easy, no need to rotate, I just need scale it along the ball's radius.
 Draw the cylinder which connects two shape balls is more difficult, I need do some math to rotate it. [Here](http://www.thjsmith.com/40/cylinder-between-two-points-opengl-c) described it.  
diff --git a/data/bmesh_test_1.h b/data/bmesh_test_1.h
index 937b74fd..de88444f 100644
--- a/data/bmesh_test_1.h
+++ b/data/bmesh_test_1.h
@@ -1,4 +1,4 @@
-const float bmeshTest1Nodes[][6] = {
+const float bmeshTest1Balls[][6] = {
   {0, -2.07575, 1.53902, 0.04122, 0.25},                                          {1, 2.40837, 2.34882, 0.48585, 0.3},
       {2, -0.91403, 0.77069, 0.62299, 0.5},            {3, 2.25224, 0.74973, 0.85115, 0.5},
                                     {4, 0, 0, 0, 0.8, 1},
@@ -6,7 +6,7 @@ const float bmeshTest1Nodes[][6] = {
                                {6, 0.01726, -0.88224, -2.87471, 0.2}
 };
 
-const int bmeshTest1Edges[][2] = {
+const int bmeshTest1Bones[][2] = {
   {0, 2}, {2, 4}, {4, 3}, {3, 1},
   {4, 5}, {5, 6}
 };
diff --git a/src/bmesh.c b/src/bmesh.c
index 57423e8b..767aa5ce 100644
--- a/src/bmesh.c
+++ b/src/bmesh.c
@@ -6,18 +6,21 @@
 #include "bmesh.h"
 #include "array.h"
 #include "matrix.h"
+#include "draw.h"
 
-typedef struct bmeshNodeIndex {
-  int nodeIndex;
+#define BMESH_STEP_DISTANCE 0.4
+
+typedef struct bmeshBallIndex {
+  int ballIndex;
   int nextChildIndex;
-} bmeshNodeIndex;
+} bmeshBallIndex;
 
 struct bmesh {
-  array *nodeArray;
-  array *edgeArray;
+  array *ballArray;
+  array *boneArray;
   array *indexArray;
   array *quadArray;
-  int rootNodeIndex;
+  int rootBallIndex;
   int roundColor;
 };
 
@@ -27,21 +30,21 @@ bmesh *bmeshCreate(void) {
     fprintf(stderr, "%s:Insufficient memory.\n", __FUNCTION__);
     return 0;
   }
-  bm->nodeArray = arrayCreate(sizeof(bmeshNode));
-  if (!bm->nodeArray) {
-    fprintf(stderr, "%s:arrayCreate bmeshNode failed.\n", __FUNCTION__);
+  bm->ballArray = arrayCreate(sizeof(bmeshBall));
+  if (!bm->ballArray) {
+    fprintf(stderr, "%s:arrayCreate bmeshBall failed.\n", __FUNCTION__);
     bmeshDestroy(bm);
     return 0;
   }
-  bm->edgeArray = arrayCreate(sizeof(bmeshEdge));
-  if (!bm->edgeArray) {
-    fprintf(stderr, "%s:arrayCreate bmeshEdge failed.\n", __FUNCTION__);
+  bm->boneArray = arrayCreate(sizeof(bmeshBone));
+  if (!bm->boneArray) {
+    fprintf(stderr, "%s:arrayCreate bmeshBone failed.\n", __FUNCTION__);
     bmeshDestroy(bm);
     return 0;
   }
-  bm->indexArray = arrayCreate(sizeof(bmeshNodeIndex));
+  bm->indexArray = arrayCreate(sizeof(bmeshBallIndex));
   if (!bm->indexArray) {
-    fprintf(stderr, "%s:arrayCreate bmeshNodeIndex failed.\n", __FUNCTION__);
+    fprintf(stderr, "%s:arrayCreate bmeshBallIndex failed.\n", __FUNCTION__);
     bmeshDestroy(bm);
     return 0;
   }
@@ -51,125 +54,128 @@ bmesh *bmeshCreate(void) {
     bmeshDestroy(bm);
     return 0;
   }
-  bm->rootNodeIndex = -1;
+  bm->rootBallIndex = -1;
   bm->roundColor = 0;
   return bm;
 }
 
 void bmeshDestroy(bmesh *bm) {
-  arrayDestroy(bm->nodeArray);
-  arrayDestroy(bm->edgeArray);
+  arrayDestroy(bm->ballArray);
+  arrayDestroy(bm->boneArray);
   arrayDestroy(bm->indexArray);
   arrayDestroy(bm->quadArray);
   free(bm);
 }
 
-int bmeshGetNodeNum(bmesh *bm) {
-  return arrayGetLength(bm->nodeArray);
+int bmeshGetBallNum(bmesh *bm) {
+  return arrayGetLength(bm->ballArray);
 }
 
-int bmeshGetEdgeNum(bmesh *bm) {
-  return arrayGetLength(bm->edgeArray);
+int bmeshGetBoneNum(bmesh *bm) {
+  return arrayGetLength(bm->boneArray);
 }
 
-bmeshNode *bmeshGetNode(bmesh *bm, int index) {
-  return (bmeshNode *)arrayGetItem(bm->nodeArray, index);
+bmeshBall *bmeshGetBall(bmesh *bm, int index) {
+  return (bmeshBall *)arrayGetItem(bm->ballArray, index);
 }
 
-bmeshEdge *bmeshGetEdge(bmesh *bm, int index) {
-  return (bmeshEdge *)arrayGetItem(bm->edgeArray, index);
+bmeshBone *bmeshGetBone(bmesh *bm, int index) {
+  return (bmeshBone *)arrayGetItem(bm->boneArray, index);
 }
 
-int bmeshAddNode(bmesh *bm, bmeshNode *node) {
-  int index = arrayGetLength(bm->nodeArray);
-  node->index = index;
-  node->firstChildIndex = -1;
-  node->childrenIndices = 0;
-  if (0 != arraySetLength(bm->nodeArray, index + 1)) {
+int bmeshAddBall(bmesh *bm, bmeshBall *ball) {
+  int index = arrayGetLength(bm->ballArray);
+  ball->index = index;
+  ball->firstChildIndex = -1;
+  ball->childrenIndices = 0;
+  if (0 != arraySetLength(bm->ballArray, index + 1)) {
     fprintf(stderr, "%s:arraySetLength failed.\n", __FUNCTION__);
     return -1;
   }
-  memcpy(arrayGetItem(bm->nodeArray, index), node, sizeof(bmeshNode));
-  if (BMESH_NODE_TYPE_ROOT == node->type) {
-    bm->rootNodeIndex = index;
+  memcpy(arrayGetItem(bm->ballArray, index), ball, sizeof(bmeshBall));
+  if (BMESH_BALL_TYPE_ROOT == ball->type) {
+    bm->rootBallIndex = index;
   }
   return index;
 }
 
-static int bmeshAddChildNodeRelation(bmesh *bm, int parentNodeIndex,
-    int childNodeIndex) {
-  bmeshNode *parentNode = bmeshGetNode(bm, parentNodeIndex);
-  bmeshNodeIndex *indexItem;
+static int bmeshAddChildBallRelation(bmesh *bm, int parentBallIndex,
+    int childBallIndex) {
+  bmeshBall *parentBall = bmeshGetBall(bm, parentBallIndex);
+  bmeshBallIndex *indexItem;
   int newChildIndex = arrayGetLength(bm->indexArray);
   if (0 != arraySetLength(bm->indexArray, newChildIndex + 1)) {
     fprintf(stderr, "%s:arraySetLength failed.\n", __FUNCTION__);
     return -1;
   }
-  indexItem = (bmeshNodeIndex *)arrayGetItem(bm->indexArray, newChildIndex);
-  indexItem->nodeIndex = childNodeIndex;
-  indexItem->nextChildIndex = parentNode->firstChildIndex;
-  parentNode->firstChildIndex = newChildIndex;
-  parentNode->childrenIndices++;
+  indexItem = (bmeshBallIndex *)arrayGetItem(bm->indexArray, newChildIndex);
+  indexItem->ballIndex = childBallIndex;
+  indexItem->nextChildIndex = parentBall->firstChildIndex;
+  parentBall->firstChildIndex = newChildIndex;
+  parentBall->childrenIndices++;
   return 0;
 }
 
-int bmeshAddEdge(bmesh *bm, bmeshEdge *edge) {
-  int index = arrayGetLength(bm->edgeArray);
-  edge->index = index;
-  if (0 != arraySetLength(bm->edgeArray, index + 1)) {
+int bmeshAddBone(bmesh *bm, bmeshBone *bone) {
+  int index = arrayGetLength(bm->boneArray);
+  bone->index = index;
+  if (0 != arraySetLength(bm->boneArray, index + 1)) {
     fprintf(stderr, "%s:arraySetLength failed.\n", __FUNCTION__);
     return -1;
   }
-  memcpy(arrayGetItem(bm->edgeArray, index), edge, sizeof(bmeshEdge));
-  if (0 != bmeshAddChildNodeRelation(bm, edge->firstNodeIndex,
-      edge->secondNodeIndex)) {
-    fprintf(stderr, "%s:bmeshAddChildNodeRelation failed.\n", __FUNCTION__);
+  memcpy(arrayGetItem(bm->boneArray, index), bone, sizeof(bmeshBone));
+  if (0 != bmeshAddChildBallRelation(bm, bone->firstBallIndex,
+      bone->secondBallIndex)) {
+    fprintf(stderr, "%s:bmeshAddChildBallRelation failed.\n", __FUNCTION__);
     return -1;
   }
-  if (0 != bmeshAddChildNodeRelation(bm, edge->secondNodeIndex,
-      edge->firstNodeIndex)) {
-    fprintf(stderr, "%s:bmeshAddChildNodeRelation failed.\n", __FUNCTION__);
+  if (0 != bmeshAddChildBallRelation(bm, bone->secondBallIndex,
+      bone->firstBallIndex)) {
+    fprintf(stderr, "%s:bmeshAddChildBallRelation failed.\n", __FUNCTION__);
     return -1;
   }
   return index;
 }
 
-static int bmeshAddInbetweenNodeBetween(bmesh *bm,
-    bmeshNode *firstNode, bmeshNode *secondNode, float frac,
-    int parentNodeIndex) {
-  bmeshNode newNode;
-  memset(&newNode, 0, sizeof(newNode));
-  newNode.type = BMESH_NODE_TYPE_INBETWEEN;
-  newNode.radius = firstNode->radius * (1 - frac) +
-    secondNode->radius * frac;
-  vec3Lerp(&firstNode->position, &secondNode->position, frac,
-    &newNode.position);
-  if (-1 == bmeshAddNode(bm, &newNode)) {
-    fprintf(stderr, "%s:bmeshAddNode failed.\n", __FUNCTION__);
+static int bmeshAddInbetweenBallBetween(bmesh *bm,
+    bmeshBall *firstBall, bmeshBall *secondBall, float frac,
+    int parentBallIndex) {
+  bmeshBall newBall;
+  memset(&newBall, 0, sizeof(newBall));
+  newBall.type = BMESH_BALL_TYPE_INBETWEEN;
+  newBall.radius = firstBall->radius * (1 - frac) +
+    secondBall->radius * frac;
+  vec3Lerp(&firstBall->position, &secondBall->position, frac,
+    &newBall.position);
+  if (-1 == bmeshAddBall(bm, &newBall)) {
+    fprintf(stderr, "%s:bmeshAddBall failed.\n", __FUNCTION__);
     return -1;
   }
-  if (-1 == bmeshAddChildNodeRelation(bm, parentNodeIndex, newNode.index)) {
-    fprintf(stderr, "%s:bmeshAddChildNodeRelation failed.\n", __FUNCTION__);
+  if (-1 == bmeshAddChildBallRelation(bm, parentBallIndex, newBall.index)) {
+    fprintf(stderr, "%s:bmeshAddChildBallRelation failed.\n", __FUNCTION__);
     return -1;
   }
-  return newNode.index;
+  return newBall.index;
 }
 
-static int bmeshGenerateNodeQuad(bmesh *bm, bmeshNode *node,
-    vec3 *localYaxis, vec3 *localZaxis, int connectWithQuad) {
-  int i;
-  quad q;
+/*
+static int bmeshGenerateBallCrossSection(bmesh *bm, bmeshBall *ball,
+    vec3 *boneDirection, vec3 *localYaxis, vec3 *localZaxis) {
+  //int i;
+  //quad q;
   vec3 z, y;
-  vec3Scale(localYaxis, node->radius, &y);
-  vec3Scale(localZaxis, node->radius, &z);
-  vec3Sub(&node->position, &y, &q.pt[0]);
+  //vec3Scale(localYaxis, ball->radius, &y);
+  //vec3Scale(localZaxis, ball->radius, &z);
+  vec3Sub(&ball->position, &y, &q.pt[0]);
   vec3Add(&q.pt[0], &z, &q.pt[0]);
-  vec3Sub(&node->position, &y, &q.pt[1]);
+  vec3Sub(&ball->position, &y, &q.pt[1]);
   vec3Sub(&q.pt[1], &z, &q.pt[1]);
-  vec3Add(&node->position, &y, &q.pt[2]);
+  vec3Add(&ball->position, &y, &q.pt[2]);
   vec3Sub(&q.pt[2], &z, &q.pt[2]);
-  vec3Add(&node->position, &y, &q.pt[3]);
+  vec3Add(&ball->position, &y, &q.pt[3]);
   vec3Add(&q.pt[3], &z, &q.pt[3]);
+  ball->crossSection = q;
+  ball->boneDirection = *boneDirection;
   if (-1 == bmeshAddQuad(bm, &q)) {
     fprintf(stderr, "%s:meshAddQuad failed.\n", __FUNCTION__);
     return -1;
@@ -197,95 +203,110 @@ static int bmeshGenerateNodeQuad(bmesh *bm, bmeshNode *node,
     }
   }
   return 0;
-}
+}*/
 
-static int bmeshGenerateInbetweenNodesBetween(bmesh *bm,
-      int firstNodeIndex, int secondNodeIndex) {
-  float step = 0.5;
+static int bmeshGenerateInbetweenBallsBetween(bmesh *bm,
+      int firstBallIndex, int secondBallIndex) {
+  float step;
   float distance;
-  int parentNodeIndex = firstNodeIndex;
+  int parentBallIndex = firstBallIndex;
   vec3 localZaxis;
   vec3 localYaxis;
-  vec3 edgeDirection;
-  vec3 worldZaxis = {0, 0, 1};
-  int lastQuadIndex = -1;
-  
-  bmeshNode *firstNode = bmeshGetNode(bm, firstNodeIndex);
-  bmeshNode *secondNode = bmeshGetNode(bm, secondNodeIndex);
-  bmeshNode *newNode;
-  if (secondNode->roundColor == bm->roundColor) {
+  vec3 boneDirection;
+  vec3 normalizedBoneDirection;
+  vec3 worldYaxis = {0, 1, 0};
+  bmeshBall *firstBall = bmeshGetBall(bm, firstBallIndex);
+  bmeshBall *secondBall = bmeshGetBall(bm, secondBallIndex);
+  bmeshBall *newBall;
+  if (secondBall->roundColor == bm->roundColor) {
     return 0;
   }
-  vec3Sub(&firstNode->position, &secondNode->position, &edgeDirection);
-  vec3CrossProduct(&worldZaxis, &edgeDirection, &localZaxis);
-  vec3Normalize(&localZaxis);
-  vec3CrossProduct(&localZaxis, &edgeDirection, &localYaxis);
-  vec3Normalize(&localYaxis);
   
-  distance = vec3Length(&edgeDirection);
-  if (distance > 0) {
-    float offset = step;
-    if (offset + step <= distance) {
-      while (offset + step <= distance) {
+  step = BMESH_STEP_DISTANCE;
+  
+  vec3Sub(&firstBall->position, &secondBall->position, &boneDirection);
+  normalizedBoneDirection = boneDirection;
+  vec3Normalize(&normalizedBoneDirection);
+  vec3CrossProduct(&worldYaxis, &boneDirection, &localYaxis);
+  vec3Normalize(&localYaxis);
+  vec3CrossProduct(&localYaxis, &boneDirection, &localZaxis);
+  vec3Normalize(&localZaxis);
+
+  distance = vec3Length(&boneDirection);
+  if (distance > BMESH_STEP_DISTANCE) {
+    float offset;
+    int calculatedStepCount = (int)(distance / BMESH_STEP_DISTANCE);
+    float remaining = distance - BMESH_STEP_DISTANCE * calculatedStepCount;
+    step += remaining / calculatedStepCount;
+    offset = step;
+    if (offset < distance) {
+      while (offset < distance) {
         float frac = offset / distance;
-        parentNodeIndex = bmeshAddInbetweenNodeBetween(bm,
-          firstNode, secondNode, frac, parentNodeIndex);
-        if (-1 == parentNodeIndex) {
+        parentBallIndex = bmeshAddInbetweenBallBetween(bm,
+          firstBall, secondBall, frac, parentBallIndex);
+        if (-1 == parentBallIndex) {
           return -1;
         }
-        newNode = bmeshGetNode(bm, parentNodeIndex);
-        bmeshGenerateNodeQuad(bm, newNode, &localYaxis, &localZaxis,
-          lastQuadIndex);
-        lastQuadIndex = -1 == lastQuadIndex ? bmeshGetQuadNum(bm) - 1 :
-          bmeshGetQuadNum(bm) - 5;
+        newBall = bmeshGetBall(bm, parentBallIndex);
+        newBall->localYaxis = localYaxis;
+        newBall->localZaxis = localZaxis;
+        newBall->boneDirection = normalizedBoneDirection;
         offset += step;
       }
     } else if (distance > step) {
-      parentNodeIndex = bmeshAddInbetweenNodeBetween(bm, firstNode, secondNode,
-        0.5, parentNodeIndex);
-      if (-1 == parentNodeIndex) {
+      parentBallIndex = bmeshAddInbetweenBallBetween(bm, firstBall, secondBall,
+        0.5, parentBallIndex);
+      if (-1 == parentBallIndex) {
         return -1;
       }
-      newNode = bmeshGetNode(bm, parentNodeIndex);
-      bmeshGenerateNodeQuad(bm, newNode, &localYaxis, &localZaxis, lastQuadIndex);
-      lastQuadIndex = -1 == lastQuadIndex ? bmeshGetQuadNum(bm) - 1 :
-          bmeshGetQuadNum(bm) - 5;
+      newBall = bmeshGetBall(bm, parentBallIndex);
+      newBall->localYaxis = localYaxis;
+      newBall->localZaxis = localZaxis;
+      newBall->boneDirection = normalizedBoneDirection;
     }
   }
-  if (-1 == bmeshAddChildNodeRelation(bm, parentNodeIndex, secondNodeIndex)) {
-    fprintf(stderr, "%s:bmeshAddChildNodeRelation failed.\n", __FUNCTION__);
+  if (-1 == bmeshAddChildBallRelation(bm, parentBallIndex, secondBallIndex)) {
+    fprintf(stderr, "%s:bmeshAddChildBallRelation failed.\n", __FUNCTION__);
     return -1;
   }
   return 0;
 }
 
-int bmeshGetNodeNextChild(bmesh *bm, bmeshNode *node, int *childIndex) {
-  int currentChildIndex = *childIndex;
-  bmeshNodeIndex *indexItem;
-  if (-1 == currentChildIndex) {
-    if (-1 == node->firstChildIndex) {
-      return -1;
-    }
-    currentChildIndex = node->firstChildIndex;
-  }
-  indexItem = (bmeshNodeIndex *)arrayGetItem(bm->indexArray, currentChildIndex);
-  *childIndex = indexItem->nextChildIndex;
-  return indexItem->nodeIndex;
-}
-
-bmeshNode *bmeshGetRootNode(bmesh *bm) {
-  if (-1 == bm->rootNodeIndex) {
+bmeshBall *bmeshGetBallFirstChild(bmesh *bm, bmeshBall *ball,
+    bmeshBallIterator *iterator) {
+  if (-1 == ball->firstChildIndex) {
     return 0;
   }
-  return bmeshGetNode(bm, bm->rootNodeIndex);
+  *iterator = ball->firstChildIndex;
+  return bmeshGetBallNextChild(bm, ball, iterator);
 }
 
-static int bmeshGenerateInbetweenNodesFrom(bmesh *bm, int parentNodeIndex) {
-  int childIndex = -1;
-  int nodeIndex;
-  bmeshNode *parent;
+bmeshBall *bmeshGetBallNextChild(bmesh *bm, bmeshBall *ball,
+    bmeshBallIterator *iterator) {
+  bmeshBallIndex *indexItem;
+  if (-1 == *iterator) {
+    return 0;
+  }
+  indexItem = (bmeshBallIndex *)arrayGetItem(bm->indexArray, *iterator);
+  *iterator = indexItem->nextChildIndex;
+  return bmeshGetBall(bm, indexItem->ballIndex);
+}
 
-  parent = bmeshGetNode(bm, parentNodeIndex);
+bmeshBall *bmeshGetRootBall(bmesh *bm) {
+  if (-1 == bm->rootBallIndex) {
+    return 0;
+  }
+  return bmeshGetBall(bm, bm->rootBallIndex);
+}
+
+static int bmeshGenerateInbetweenBallsFrom(bmesh *bm, int parentBallIndex) {
+  bmeshBallIterator iterator;
+  int ballIndex;
+  bmeshBall *parent;
+  bmeshBall *ball;
+  int oldChildrenIndices;
+
+  parent = bmeshGetBall(bm, parentBallIndex);
   if (parent->roundColor == bm->roundColor) {
     return 0;
   }
@@ -293,30 +314,30 @@ static int bmeshGenerateInbetweenNodesFrom(bmesh *bm, int parentNodeIndex) {
 
   //
   // Old indices came from user's input will be removed
-  // after the inbetween nodes are genereated, though
+  // after the inbetween balls are genereated, though
   // the space occupied in indexArray will not been release.
   //
 
-  childIndex = parent->firstChildIndex;
+  ball = bmeshGetBallFirstChild(bm, parent, &iterator);
   parent->firstChildIndex = -1;
+  oldChildrenIndices = parent->childrenIndices;
   parent->childrenIndices = 0;
-
-  while (-1 != childIndex) {
-    nodeIndex = bmeshGetNodeNextChild(bm, parent, &childIndex);
-    if (-1 == nodeIndex) {
-      break;
-    }
-    if (0 != bmeshGenerateInbetweenNodesBetween(bm, parentNodeIndex,
-        nodeIndex)) {
+  
+  for (;
+      ball;
+      ball = bmeshGetBallNextChild(bm, parent, &iterator)) {
+    ballIndex = ball->index;
+    if (0 != bmeshGenerateInbetweenBallsBetween(bm, parentBallIndex,
+        ballIndex)) {
       fprintf(stderr,
-        "%s:bmeshGenerateInbetweenNodesBetween failed(parentNodeIndex:%d).\n",
-        __FUNCTION__, parentNodeIndex);
+        "%s:bmeshGenerateInbetweenBallsBetween failed(parentBallIndex:%d).\n",
+        __FUNCTION__, parentBallIndex);
       return -1;
     }
-    if (0 != bmeshGenerateInbetweenNodesFrom(bm, nodeIndex)) {
+    if (0 != bmeshGenerateInbetweenBallsFrom(bm, ballIndex)) {
       fprintf(stderr,
-        "%s:bmeshGenerateInbetweenNodesFrom failed(nodeIndex:%d).\n",
-        __FUNCTION__, nodeIndex);
+        "%s:bmeshGenerateInbetweenBallsFrom failed(ballIndex:%d).\n",
+        __FUNCTION__, ballIndex);
       return -1;
     }
   }
@@ -324,13 +345,13 @@ static int bmeshGenerateInbetweenNodesFrom(bmesh *bm, int parentNodeIndex) {
   return 0;
 }
 
-int bmeshGenerateInbetweenNodes(bmesh *bm) {
-  if (-1 == bm->rootNodeIndex) {
-    fprintf(stderr, "%s:No root node.\n", __FUNCTION__);
+int bmeshGenerateInbetweenBalls(bmesh *bm) {
+  if (-1 == bm->rootBallIndex) {
+    fprintf(stderr, "%s:No root ball.\n", __FUNCTION__);
     return -1;
   }
   bm->roundColor++;
-  return bmeshGenerateInbetweenNodesFrom(bm, bm->rootNodeIndex);
+  return bmeshGenerateInbetweenBallsFrom(bm, bm->rootBallIndex);
 }
 
 int bmeshGetQuadNum(bmesh *bm) {
@@ -351,4 +372,131 @@ int bmeshAddQuad(bmesh *bm, quad *q) {
   return index;
 }
 
+static int bmeshSweepFrom(bmesh *bm, bmeshBall *parent, bmeshBall *ball) {
+  int result = 0;
+  vec3 worldYaxis = {0, 1, 0};
+  bmeshBallIterator iterator;
+  bmeshBall *child = 0;
+  if (BMESH_BALL_TYPE_KEY == ball->type) {
+    child = bmeshGetBallFirstChild(bm, ball, &iterator);
+    if (child) {
+      if (parent) {
+        float rotateAngle;
+        vec3 rotateAxis;
+        vec3CrossProduct(&parent->boneDirection, &child->boneDirection,
+          &rotateAxis);
+        vec3Normalize(&rotateAxis);
+        vec3Add(&rotateAxis, &ball->position, &rotateAxis);
+        rotateAngle = vec3Angle(&parent->boneDirection, &child->boneDirection);
+        rotateAngle *= 0.5;
+        
+        /*
+        glColor3f(0.0, 0.0, 0.0);
+        drawDebugPrintf("<%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,
+          rotateAngle);
+        
+        glPushMatrix();
+        glTranslatef(parent->position.x, parent->position.y, parent->position.z);
+        glColor3f(1.0, 0.0, 1.0);
+        glBegin(GL_LINES);
+        glVertex3f(0, 0, 0);
+        glVertex3f(parent->boneDirection.x, parent->boneDirection.y,
+          parent->boneDirection.z);
+        glEnd();
+        glPopMatrix();
+        
+        glPushMatrix();
+        glTranslatef(child->position.x, child->position.y, child->position.z);
+        glColor3f(0.0, 1.0, 1.0);
+        glBegin(GL_LINES);
+        glVertex3f(0, 0, 0);
+        glVertex3f(child->boneDirection.x, child->boneDirection.y,
+          child->boneDirection.z);
+        glEnd();
+        glPopMatrix();
+        */
+        
+        ball->boneDirection = parent->boneDirection;
+        vec3RotateAlong(&ball->boneDirection, rotateAngle, &rotateAxis,
+          &ball->boneDirection);
+        vec3CrossProduct(&worldYaxis, &ball->boneDirection, &ball->localYaxis);
+        vec3Normalize(&ball->localYaxis);
+        vec3CrossProduct(&ball->localYaxis, &ball->boneDirection,
+          &ball->localZaxis);
+        vec3Normalize(&ball->localZaxis);
+      } else {
+        // TODO:
+      }
+    } else {
+      ball->boneDirection = parent->boneDirection;
+      vec3CrossProduct(&worldYaxis, &ball->boneDirection, &ball->localYaxis);
+      vec3Normalize(&ball->localYaxis);
+      vec3CrossProduct(&ball->localYaxis, &ball->boneDirection,
+        &ball->localZaxis);
+      vec3Normalize(&ball->localZaxis);
+    }
+  }
+  for (child = bmeshGetBallFirstChild(bm, ball, &iterator);
+      child;
+      child = bmeshGetBallNextChild(bm, ball, &iterator)) {
+    result = bmeshSweepFrom(bm, ball, child);
+    if (0 != result) {
+      fprintf(stderr, "%s:bmeshSweepFrom failed.\n", __FUNCTION__);
+      return result;
+    }
+  }
+  return result;
+}
 
+int bmeshSweep(bmesh *bm) {
+  return bmeshSweepFrom(bm, 0, bmeshGetRootBall(bm));
+}
+
+static bmeshBall *bmeshFindBallForConvexHull(bmesh *bm, bmeshBall *root,
+      bmeshBall *ball) {
+  bmeshBallIterator iterator;
+  bmeshBall *child;
+  float distance = vec3Distance(&root->position, &ball->position);
+  if (distance >= root->radius) {
+    return ball;
+  }
+  child = bmeshGetBallFirstChild(bm, ball, &iterator);
+  if (!child) {
+    return ball;
+  }
+  return bmeshFindBallForConvexHull(bm, root, child);
+}
+
+static int bmeshStichFrom(bmesh *bm, bmeshBall *ball) {
+  int result = 0;
+  bmeshBallIterator iterator;
+  bmeshBall *child;
+  bmeshBall *ballForConvexHull;
+  if (BMESH_BALL_TYPE_ROOT == ball->type) {
+    for (child = bmeshGetBallFirstChild(bm, ball, &iterator);
+        child;
+        child = bmeshGetBallNextChild(bm, ball, &iterator)) {
+      ballForConvexHull = bmeshFindBallForConvexHull(bm, ball, child);
+    }
+  }
+  for (child = bmeshGetBallFirstChild(bm, ball, &iterator);
+      child;
+      child = bmeshGetBallNextChild(bm, ball, &iterator)) {
+    result = bmeshSweepFrom(bm, ball, child);
+    if (0 != result) {
+      fprintf(stderr, "%s:bmeshSweepFrom failed.\n", __FUNCTION__);
+      return result;
+    }
+  }
+  return result;
+}
+
+int bmeshStitch(bmesh *bm) {
+  return bmeshStichFrom(bm, bmeshGetRootBall(bm));
+}
diff --git a/src/bmesh.h b/src/bmesh.h
index 838ad92f..329e804f 100644
--- a/src/bmesh.h
+++ b/src/bmesh.h
@@ -8,10 +8,10 @@ extern "C" {
 #endif
 
 typedef enum {
-  BMESH_NODE_TYPE_KEY = 0,
-  BMESH_NODE_TYPE_ROOT = 1,
-  BMESH_NODE_TYPE_INBETWEEN = 2,
-} bmeshNodeType;
+  BMESH_BALL_TYPE_KEY = 0,
+  BMESH_BALL_TYPE_ROOT = 1,
+  BMESH_BALL_TYPE_INBETWEEN = 2,
+} bmeshBallType;
 
 typedef struct bmesh bmesh;
 
@@ -22,29 +22,39 @@ typedef struct {
   int type;
   int childrenIndices;
   int firstChildIndex;
+  vec3 boneDirection;
+  vec3 localYaxis;
+  vec3 localZaxis;
   int roundColor;
-} bmeshNode;
+} bmeshBall;
+
+typedef int bmeshBallIterator;
 
 typedef struct {
   int index;
-  int firstNodeIndex;
-  int secondNodeIndex;
-} bmeshEdge;
+  int firstBallIndex;
+  int secondBallIndex;
+} bmeshBone;
 
 bmesh *bmeshCreate(void);
 void bmeshDestroy(bmesh *bm);
-int bmeshGetNodeNum(bmesh *bm);
-int bmeshGetEdgeNum(bmesh *bm);
-bmeshNode *bmeshGetNode(bmesh *bm, int index);
-bmeshEdge *bmeshGetEdge(bmesh *bm, int index);
-int bmeshAddNode(bmesh *bm, bmeshNode *node);
-int bmeshAddEdge(bmesh *bm, bmeshEdge *edge);
-int bmeshGenerateInbetweenNodes(bmesh *bm);
-int bmeshGetNodeNextChild(bmesh *bm, bmeshNode *node, int *childIndex);
-bmeshNode *bmeshGetRootNode(bmesh *bm);
+int bmeshGetBallNum(bmesh *bm);
+int bmeshGetBoneNum(bmesh *bm);
+bmeshBall *bmeshGetBall(bmesh *bm, int index);
+bmeshBone *bmeshGetBone(bmesh *bm, int index);
+int bmeshAddBall(bmesh *bm, bmeshBall *ball);
+int bmeshAddBone(bmesh *bm, bmeshBone *bone);
+int bmeshGenerateInbetweenBalls(bmesh *bm);
+bmeshBall *bmeshGetBallFirstChild(bmesh *bm, bmeshBall *node,
+  bmeshBallIterator *iterator);
+bmeshBall *bmeshGetBallNextChild(bmesh *bm, bmeshBall *node,
+  bmeshBallIterator *iterator);
+bmeshBall *bmeshGetRootBall(bmesh *bm);
 int bmeshGetQuadNum(bmesh *bm);
 quad *bmeshGetQuad(bmesh *bm, int index);
 int bmeshAddQuad(bmesh *bm, quad *q);
+int bmeshSweep(bmesh *bm);
+int bmeshStitch(bmesh *bm);
 
 #ifdef __cplusplus
 }
diff --git a/src/draw.cpp b/src/draw.cpp
index f685e90d..85dce4d4 100644
--- a/src/draw.cpp
+++ b/src/draw.cpp
@@ -97,3 +97,4 @@ int drawPrintf(int x, int y, const char *fmt, ...) {
   vsnprintf(text, sizeof(text), fmt, args);
   return drawText(x, y, text);
 }
+
diff --git a/src/draw.h b/src/draw.h
index bfd524fc..e623c439 100644
--- a/src/draw.h
+++ b/src/draw.h
@@ -32,6 +32,7 @@ int drawCylinder(vec3 *topOrigin, vec3 *bottomOrigin, float radius, int slices,
 int drawGrid(float size, float step);
 int drawText(int x, int y, char *text);
 int drawPrintf(int x, int y, const char *fmt, ...);
+int drawDebugPrintf(const char *fmt, ...);
 
 #ifdef __cplusplus
 }
diff --git a/src/render.cpp b/src/render.cpp
index 01ef8023..e113d6ed 100644
--- a/src/render.cpp
+++ b/src/render.cpp
@@ -6,62 +6,108 @@
 #include "draw.h"
 #include "bmesh.h"
 #include "matrix.h"
+#include "vector3d.h"
 
-static const float bmeshNodeColors[][3] {
+static const float bmeshBallColors[][3] {
   {0, 0.78, 1},
   {1, 0, 0},
   {1, 1, 1}
 };
 
-static const float bmeshEdgeColor[3] = {1, 1, 0};
+static const float bmeshBoneColor[3] = {1, 1, 0};
 
 static QGLWidget *_this = 0;
+static int debugOutputTop = 0;
 
-static int drawBmeshNode(bmesh *bm, bmeshNode *node) {
-  glColor3fv(bmeshNodeColors[node->type]);
-  drawSphere(&node->position, node->radius, 36, 24);
+int drawDebugPrintf(const char *fmt, ...) {
+  int x = 0;
+  int y = debugOutputTop + 10;
+  va_list args;
+  char text[1024];
+  va_start(args, fmt);
+  vsnprintf(text, sizeof(text), fmt, args);
+  debugOutputTop += 9;
+  return drawText(x, y, text);
+}
+
+static int drawBmeshBall(bmesh *bm, bmeshBall *ball) {
+  glColor3fv(bmeshBallColors[ball->type]);
+  drawSphere(&ball->position, ball->radius, 36, 24);
   return 0;
 }
 
-static void drawBmeshNodeRecursively(bmesh *bm, bmeshNode *node) {
-  int childIndex = node->firstChildIndex;
-  int childNodeIndex;
-  drawBmeshNode(bm, node);
-  while (-1 != childIndex) {
-    childNodeIndex = bmeshGetNodeNextChild(bm, node, &childIndex);
-    if (-1 == childNodeIndex) {
-      break;
-    }
-    drawBmeshNodeRecursively(bm, bmeshGetNode(bm, childNodeIndex));
+static void drawBmeshBallRecursively(bmesh *bm, bmeshBall *ball) {
+  bmeshBallIterator iterator;
+  bmeshBall *child;
+  drawBmeshBall(bm, ball);
+  for (child = bmeshGetBallFirstChild(bm, ball, &iterator);
+      child;
+      child = bmeshGetBallNextChild(bm, ball, &iterator)) {
+    drawBmeshBallRecursively(bm, child);
   }
 }
 
-static void drawBmeshNodeQuardRecursively(bmesh *bm, bmeshNode *node) {
-  int childIndex = node->firstChildIndex;
-  int childNodeIndex;
+static int drawBmeshBallQuad(bmeshBall *ball) {
+  vec3 normal;
+  int j;
+  vec3 z, y;
+  quad q;
   
+  vec3Scale(&ball->localYaxis, ball->radius, &y);
+  vec3Scale(&ball->localZaxis, ball->radius, &z);
+  vec3Sub(&ball->position, &y, &q.pt[0]);
+  vec3Add(&q.pt[0], &z, &q.pt[0]);
+  vec3Sub(&ball->position, &y, &q.pt[1]);
+  vec3Sub(&q.pt[1], &z, &q.pt[1]);
+  vec3Add(&ball->position, &y, &q.pt[2]);
+  vec3Sub(&q.pt[2], &z, &q.pt[2]);
+  vec3Add(&ball->position, &y, &q.pt[3]);
+  vec3Add(&q.pt[3], &z, &q.pt[3]);
+  
+  glColor4f(1.0f, 1.0f, 1.0f, 0.5);
+  glBegin(GL_QUADS);
+  vec3Normal(&q.pt[0], &q.pt[1], &q.pt[2], &normal);
+  for (j = 0; j < 4; ++j) {
+    glNormal3f(normal.x, normal.y, normal.z);
+    glVertex3f(q.pt[j].x, q.pt[j].y, q.pt[j].z);
+  }
+  glEnd();
+  
+  glColor3f(0.0f, 0.0f, 0.0f);
+  glBegin(GL_LINE_STRIP);
+  for (j = 0; j < 4; ++j) {
+    glVertex3f(q.pt[j].x, q.pt[j].y, q.pt[j].z);
+  }
+  glVertex3f(q.pt[0].x, q.pt[0].y, q.pt[0].z);
+  glEnd();
+}
+
+static void drawBmeshBallQuadRecursively(bmesh *bm, bmeshBall *ball) {
+  bmeshBallIterator iterator;
+  bmeshBall *child;
+
   /*
   matrix matTmp;
   matrix matCalc;
   float quad[4][3] = {
-    {-node->radius, +node->radius, 0},
-    {-node->radius, -node->radius, 0},
-    {+node->radius, -node->radius, 0},
-    {+node->radius, +node->radius, 0},
+    {-ball->radius, +ball->radius, 0},
+    {-ball->radius, -ball->radius, 0},
+    {+ball->radius, -ball->radius, 0},
+    {+ball->radius, +ball->radius, 0},
   };
   matrixLoadIdentity(&matCalc);
   matrixAppend(&matCalc,
-    matrixTranslate(&matTmp, node->position.x, node->position.y,
-      node->position.z));
+    matrixTranslate(&matTmp, ball->position.x, ball->position.y,
+      ball->position.z));
   matrixAppend(&matCalc,
     matrixRotate(&matTmp,
-      node->rotateAngle, node->rotateAround.x, node->rotateAround.y,
-      node->rotateAround.z));
+      ball->rotateAngle, ball->rotateAround.x, ball->rotateAround.y,
+      ball->rotateAround.z));
   matrixTransformVector(&matCalc, quad[0]);
   matrixTransformVector(&matCalc, quad[1]);
   matrixTransformVector(&matCalc, quad[2]);
   matrixTransformVector(&matCalc, quad[3]);
-  
+
   glVertex3fv(quad[0]);
   glVertex3fv(quad[1]);
   glVertex3fv(quad[2]);
@@ -70,33 +116,33 @@ static void drawBmeshNodeQuardRecursively(bmesh *bm, bmeshNode *node) {
 
   /*
   glPushMatrix();
-  glTranslatef(node->position.x, node->position.y,
-    node->position.z);
-  glRotatef(node->rotateAngle, node->rotateAround.x, node->rotateAround.y,
-    node->rotateAround.z);
+  glTranslatef(ball->position.x, ball->position.y,
+    ball->position.z);
+  glRotatef(ball->rotateAngle, ball->rotateAround.x, ball->rotateAround.y,
+    ball->rotateAround.z);
   glBegin(GL_QUADS);
-    glVertex3f(-node->radius, +node->radius, 0);
-    glVertex3f(-node->radius, -node->radius, 0);
-    glVertex3f(+node->radius, -node->radius, 0);
-    glVertex3f(+node->radius, +node->radius, 0);
+    glVertex3f(-ball->radius, +ball->radius, 0);
+    glVertex3f(-ball->radius, -ball->radius, 0);
+    glVertex3f(+ball->radius, -ball->radius, 0);
+    glVertex3f(+ball->radius, +ball->radius, 0);
   glEnd();
   glPopMatrix();
   */
   
-  while (-1 != childIndex) {
-    childNodeIndex = bmeshGetNodeNextChild(bm, node, &childIndex);
-    if (-1 == childNodeIndex) {
-      break;
-    }
-    drawBmeshNodeQuardRecursively(bm, bmeshGetNode(bm, childNodeIndex));
+  drawBmeshBallQuad(ball);
+
+  for (child = bmeshGetBallFirstChild(bm, ball, &iterator);
+      child;
+      child = bmeshGetBallNextChild(bm, ball, &iterator)) {
+    drawBmeshBallQuadRecursively(bm, child);
   }
 }
 
-static int drawBmeshEdge(bmesh *bm, bmeshEdge *edge) {
-  glColor3fv(bmeshEdgeColor);
-  bmeshNode *firstNode = bmeshGetNode(bm, edge->firstNodeIndex);
-  bmeshNode *secondNode = bmeshGetNode(bm, edge->secondNodeIndex);
-  drawCylinder(&firstNode->position, &secondNode->position, 0.1, 36, 24);
+static int drawBmeshBone(bmesh *bm, bmeshBone *bone) {
+  glColor3fv(bmeshBoneColor);
+  bmeshBall *firstBall = bmeshGetBall(bm, bone->firstBallIndex);
+  bmeshBall *secondBall = bmeshGetBall(bm, bone->secondBallIndex);
+  drawCylinder(&firstBall->position, &secondBall->position, 0.1, 36, 24);
   return 0;
 }
 
@@ -137,7 +183,7 @@ void Render::initializeGL() {
   qglClearColor(QWidget::palette().color(QWidget::backgroundRole()));
   glClearStencil(0);
   glClearDepth(1.0f);
-  
+
   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 
   GLfloat ambientLight[] = {0.0f, 0.0f, 0.0f, 1.0f};
@@ -166,9 +212,10 @@ void Render::initializeGL() {
 #include "../data/bmesh_test_1.h"
 
 void Render::paintGL() {
-  static bmesh *bm = 0;
+  bmesh *bm = 0;
 
   _this = this;
+  debugOutputTop = 0;
 
   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
@@ -179,7 +226,7 @@ void Render::paintGL() {
   glRotatef(cameraAngleY, 0, 1, 0);
 
   glColor3f(0, 0, 0);
-  drawPrintf(0, 10, "cameraAngleX:%f cameraAngleY:%f cameraDistance:%f",
+  drawDebugPrintf("cameraAngleX:%f cameraAngleY:%f cameraDistance:%f",
     cameraAngleX, cameraAngleY, cameraDistance);
 
   drawGrid(10, 1);
@@ -187,47 +234,48 @@ void Render::paintGL() {
   glEnable(GL_LIGHTING);
 
   if (0 == bm) {
-    bmeshNode node;
-    bmeshEdge edge;
+    bmeshBall ball;
+    bmeshBone bone;
     int i;
     bm = bmeshCreate();
 
-    for (i = 0; i < sizeof(bmeshTest1Nodes) / sizeof(bmeshTest1Nodes[0]); ++i) {
-      memset(&node, 0, sizeof(node));
-      node.position.x = bmeshTest1Nodes[i][1];
-      node.position.y = bmeshTest1Nodes[i][2];
-      node.position.z = bmeshTest1Nodes[i][3];
-      node.radius = bmeshTest1Nodes[i][4];
-      node.type = bmeshTest1Nodes[i][5];
-      bmeshAddNode(bm, &node);
+    for (i = 0; i < sizeof(bmeshTest1Balls) / sizeof(bmeshTest1Balls[0]); ++i) {
+      memset(&ball, 0, sizeof(ball));
+      ball.position.x = bmeshTest1Balls[i][1];
+      ball.position.y = bmeshTest1Balls[i][2];
+      ball.position.z = bmeshTest1Balls[i][3];
+      ball.radius = bmeshTest1Balls[i][4];
+      ball.type = bmeshTest1Balls[i][5];
+      bmeshAddBall(bm, &ball);
     }
 
-    for (i = 0; i < sizeof(bmeshTest1Edges) / sizeof(bmeshTest1Edges[0]); ++i) {
-      memset(&edge, 0, sizeof(edge));
-      edge.firstNodeIndex = bmeshTest1Edges[i][0];
-      edge.secondNodeIndex = bmeshTest1Edges[i][1];
-      bmeshAddEdge(bm, &edge);
+    for (i = 0; i < sizeof(bmeshTest1Bones) / sizeof(bmeshTest1Bones[0]); ++i) {
+      memset(&bone, 0, sizeof(bone));
+      bone.firstBallIndex = bmeshTest1Bones[i][0];
+      bone.secondBallIndex = bmeshTest1Bones[i][1];
+      bmeshAddBone(bm, &bone);
     }
 
-    bmeshGenerateInbetweenNodes(bm);
+    bmeshGenerateInbetweenBalls(bm);
+    bmeshSweep(bm);
   }
 
-  drawBmeshNodeRecursively(bm, bmeshGetRootNode(bm));
-  
+  drawBmeshBallRecursively(bm, bmeshGetRootBall(bm));
+
   //glBegin(GL_QUADS);
-  //drawBmeshNodeQuardRecursively(bm, bmeshGetRootNode(bm));
+  drawBmeshBallQuadRecursively(bm, bmeshGetRootBall(bm));
   //glEnd();
 
   {
     int index;
     /*
-    for (index = 0; index < bmeshGetNodeNum(bm); ++index) {
-      bmeshNode *node = bmeshGetNode(bm, index);
-      drawBmeshNode(bm, node);
+    for (index = 0; index < bmeshGetBallNum(bm); ++index) {
+      bmeshBall *ball = bmeshGetBall(bm, index);
+      drawBmeshBall(bm, ball);
     }*/
-    for (index = 0; index < bmeshGetEdgeNum(bm); ++index) {
-      bmeshEdge *edge = bmeshGetEdge(bm, index);
-      drawBmeshEdge(bm, edge);
+    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);
@@ -256,6 +304,11 @@ void Render::paintGL() {
   }
 
   glPopMatrix();
+  
+  if (bm) {
+    bmeshDestroy(bm);
+    bm = 0;
+  }
 }
 
 void Render::resizeGL(int w, int h) {
diff --git a/src/vector3d.c b/src/vector3d.c
index 4c29f0c1..35d4ea49 100644
--- a/src/vector3d.c
+++ b/src/vector3d.c
@@ -1,5 +1,6 @@
 #include <math.h>
 #include "vector3d.h"
+#include "matrix.h"
 
 float vec3Length(vec3 *p) {
   double mag;
@@ -83,3 +84,44 @@ void vec3Normal(vec3 *a, vec3 *b, vec3 *c, vec3 *normal) {
   normal->y = vr[1]/val;
   normal->z = vr[2]/val;
 }
+
+void vec3RotateAlong(vec3 *a, float angle, vec3 *axis, vec3 *result) {
+  float vec[3];
+  matrix matRotate;
+  matrix matFinal;
+  matrixLoadIdentity(&matFinal);
+  matrixAppend(&matFinal, matrixRotate(&matRotate, angle,
+    axis->x, axis->y, axis->z));
+  vec[0] = a->x;
+  vec[1] = a->y;
+  vec[2] = a->z;
+  matrixTransformVector(&matFinal, vec);
+  result->x = vec[0];
+  result->y = vec[1];
+  result->z = vec[2];
+}
+
+float vec3Angle(vec3 *a, vec3 *b) {
+  float angle;
+  vec3 p;
+  float distance;
+  float dot;
+  float acosParam;
+  float acosVal;
+  vec3Sub(a, b, &p);
+  distance = vec3Length(&p);
+  if (0 == distance) {
+    return 0;
+  }
+  dot = vec3DotProduct(a, b);
+  acosParam = dot / distance;
+  if (acosParam < -1) {
+    acosParam = -1;
+  }
+  if (acosParam > 1) {
+    acosParam = 1;
+  }
+  acosVal = acos(acosParam);
+  angle = 180 / M_PI * acosVal;
+  return angle;
+}
diff --git a/src/vector3d.h b/src/vector3d.h
index 2ec1d3a9..45cd8c39 100644
--- a/src/vector3d.h
+++ b/src/vector3d.h
@@ -22,6 +22,8 @@ float vec3DotProduct(vec3 *a, vec3 *b);
 float vec3Length(vec3 *p);
 float vec3Distance(vec3 *a, vec3 *b);
 void vec3Normal(vec3 *a, vec3 *b, vec3 *c, vec3 *normal);
+void vec3RotateAlong(vec3 *a, float angle, vec3 *axis, vec3 *result);
+float vec3Angle(vec3 *a, vec3 *b);
 
 #ifdef __cplusplus
 }