Java 3D Arrow problem

Question

I have the following Java class that draws a 3D arrow in space, but there are two problems,I don't quite know how to fix them.

import javax.swing.*;
import java.awt.Frame;
import com.sun.j3d.utils.applet.MainFrame;
import com.sun.j3d.utils.universe.*;
import com.sun.j3d.utils.geometry.*;
import javax.media.j3d.*;
import javax.vecmath.*;

public class Arrow extends JApplet
{
  public static final long serialVersionUID=26362862L;

  public static TransformGroup createArrow(float ArrowDia,Vector3f stPt,Vector3f endPt,Color3f Color,String Name)      // Creates an 3D arrow between point stPt and endPt
  {
    double lenSq=(stPt.x-endPt.x)*(stPt.x-endPt.x)+(stPt.y-endPt.y)*(stPt.y-endPt.y)+(stPt.z-endPt.z)*(stPt.z-endPt.z);
    float ArrowLen=(float)Math.sqrt(lenSq);
    float ArrowHeadLen=12.0f*ArrowDia;
    float ArrowHeadDia=3.0f*ArrowDia;
    float ArrowTailDia=ArrowDia;
    float AroowTailLen=0.5f*ArrowDia;
    float CylenderLen=ArrowLen-ArrowHeadLen-AroowTailLen;

    Transform3D caTransform=new Transform3D();
    Matrix4d RotMat=new Matrix4d();                    // Rotation Matrix for whole arrow (cylinder + two cones)

    if (stPt.equals(endPt)) caTransform.setIdentity();
    else
    {
      Vector3d unitVect=new Vector3d((endPt.x-stPt.x),(endPt.y-stPt.y),(endPt.z-stPt.z));
      unitVect.normalize();
/*
        X-axis rotation           Y-axis rotation             Z-axis rotation
      -------------------------------------------------------------------------
      1     0     0     0      cosT    0   sinT    0      cosT -sinT    0     0
      0   cosT -sinT    0        0     1     0     0      sinT  cosT    0     0
      0   sinT  cosT    0     -sinT    0   cosT    0        0     0     1     0
      0     0     0     1        0     0     0     1        0     0     0     1
*/ 
      double angle_x=Math.acos(unitVect.y);                    // find rotation about X axis
      double angle_y=-0.5*unitVect.x*Math.PI;                  // find rotation about Y axis
      if (unitVect.z!=0.0) angle_y=Math.atan(unitVect.x/unitVect.z);
      if (unitVect.z<=0) angle_y+=Math.PI;

      // determine the transform matrix for X.Y axis rotation and translation to the midpoint of the line segment.

      double A=Math.cos(angle_x);
      double B=Math.sin(angle_x);
      double C=Math.cos(angle_y);
      double D=Math.sin(angle_y);
      RotMat.m00=C;
      RotMat.m01=B*D;
      RotMat.m02=A*D;
      RotMat.m03=stPt.x+0.5f*(endPt.x-stPt.x);
      RotMat.m10=0.0;
      RotMat.m11=A;
      RotMat.m12=-B;
      RotMat.m13=stPt.y+0.5f*(endPt.y-stPt.y);
      RotMat.m20=-D;
      RotMat.m21=B*C;
      RotMat.m22=A*C;
      RotMat.m23=stPt.z+0.5f*(endPt.z-stPt.z);
      RotMat.m30=0.0;
      RotMat.m31=0.0;
      RotMat.m32=0.0;
      RotMat.m33=1.0;
    }

    Appearance caAppearance=new Appearance();        // Apperance for the arrow
    ColoringAttributes caColor;
    caColor=new ColoringAttributes();
    caColor.setColor(Color);
    caAppearance.setColoringAttributes(caColor);
    caTransform.set(RotMat);

    TransformGroup caTransformGroup=new TransformGroup(caTransform);

    Node cArrowCylinder=new Cylinder(ArrowDia,CylenderLen,caAppearance);
    cArrowCylinder.setName(Name);

    Transform3D arrowHeadTransform=new Transform3D();
    arrowHeadTransform.set(new Vector3f(0.0f,0.5f*CylenderLen+0.5f*ArrowHeadLen,0.0f));
    TransformGroup arrowHeadTransformGroup=new TransformGroup(arrowHeadTransform);

    Transform3D arrowTailTransform=new Transform3D();
    arrowTailTransform.set(new Vector3f(0.0f,-0.5f*CylenderLen-0.5f*AroowTailLen,0.0f));
    Transform3D arrowTailTransformTmp=new Transform3D();
    arrowTailTransformTmp.rotZ(Math.PI);
    arrowTailTransform.mul(arrowTailTransformTmp);
    TransformGroup arrowTailTransformGroup=new TransformGroup(arrowTailTransform);

    Node ArrowHeadCone=new Cone(ArrowHeadDia,ArrowHeadLen,1,caAppearance);
    ArrowHeadCone.setName(Name);
    Node ArrowTailCone=new Cone(ArrowTailDia,AroowTailLen,1,caAppearance);
    ArrowTailCone.setName(Name);
    arrowHeadTransformGroup.addChild(ArrowHeadCone);
    arrowTailTransformGroup.addChild(ArrowTailCone);

    caTransformGroup.addChild(arrowHeadTransformGroup);
    caTransformGroup.addChild(arrowTailTransformGroup);
    caTransformGroup.addChild(cArrowCylinder);

    return caTransformGroup;
  }

  static BranchGroup create_Arrow_SceneGraph()
  {
    BranchGroup SceneRoot=new BranchGroup();

    SceneRoot.addChild(Arrow.createArrow(0.01f,new Vector3f(-0.8f,0.0f,0.0f),new Vector3f(0.8f,0.0f,0.0f),new Color3f(0.0f,0.8f,1.0f),"1"));
    SceneRoot.addChild(Arrow.createArrow(0.01f,new Vector3f(0.0f,-0.8f,0.0f),new Vector3f(0.0f,0.8f,0.0f),new Color3f(0.0f,0.8f,1.0f),"2"));
    SceneRoot.addChild(Arrow.createArrow(0.01f,new Vector3f(0.0f,0.0f,-0.8f),new Vector3f(0.0f,0.0f,0.8f),new Color3f(0.0f,0.8f,1.0f),"3"));
    SceneRoot.addChild(Arrow.createArrow(0.01f,new Vector3f(0.35f,0.25f,0.0f),new Vector3f(0.35f,-0.25f,0.0f),new Color3f(0.0f,0.8f,1.0f),"4"));

    return SceneRoot;
  }

  public static void main(String[] args)
  {
    Arrow scene=new Arrow();
    Canvas3D jCanvas3D=new Canvas3D(SimpleUniverse.getPreferredConfiguration());
    scene.getContentPane().add(jCanvas3D);
    BranchGroup jScene=create_Arrow_SceneGraph();
    jScene.compile();
    SimpleUniverse jVirtualUniverse=new SimpleUniverse(jCanvas3D);
    jVirtualUniverse.getViewingPlatform().setNominalViewingTransform();
    jVirtualUniverse.addBranchGraph(jScene);
    Frame jFrame=new MainFrame(scene,600,600);
  }
}

<1> When the starting point is 0 and end point is also 0, there is an error message.

<2> When ArrowDia gets larger, the arrow's starting point is moving away from where it is intended. You can see this effect when you draw two arrows starting from the same point, but with two different ArrowDia values, the bigger ArrowDia is, the further it is from the intended starting point, the arrow should start from exactly the same point no matter how thick it gets.

Anyone knows how to fix them ?