Crank Model Now Works

Last Friday I expanded the piston assembly model to a full cranktrain model. The original Smith method worked immediately, but Baraff method did not. Today I finally have the bug fixed :) My confidence in the Baraff method rises as the results match pretty well with those of the Smith method. Now the performace of the Baraff method is still a question mark. I don't feel the program running any faster using the new method.

The bug fix is within buildTree() in World2.java

  TreeNode buildTree(Body b) {
    // get b = the next enabled, untagged body, and tag it
    if (b.tag!=0 || (b.flags & Body.Disabled)!=0) return null;
    b.tag = 1;

    aux.clear();
    LinkedList stack = new LinkedList();
    // tag all bodies and joints starting from b
    TreeNode root = new TreeNode(b, null);
    TreeNode node = root, childnode;
    stack.addFirst(node);
    while (stack.size() > 0) {
      node = (TreeNode)stack.removeFirst();  // pop body node off stack
      b = node.body;

      // traverse and tag all body's joints,
      // add untagged connected bodies to stack
      Iterator j = b.nodes.iterator();
      while(j.hasNext()) {
        JointNode jn = (JointNode)j.next();
        if(jn.joint.tag==0) {
          jn.joint.tag = 1;
          if(jn.body!=null && jn.body.tag==0) {
            childnode = new TreeNode(jn.joint, node);
            childnode = new TreeNode(jn.body, childnode);
            jn.body.tag = 1;
            stack.addFirst(childnode);
          }
          else
            aux.add(n.joint); // put boundary joints into auxiliary joint list
        }
      }
    }
    return root;
  }