#!/usr/bin/perl
#
# bvh2melskel - convert bvh mocap file to mel commands to generate
#                the skeleton in maya (using hanim names)
#               NO motion is converted (yet - next script...)
#
# Assumes Filmbox BVH joint naming conventions.  Add your own joint
# names to the "hanimName()" function at the bottom.
#
# Should be able to find this at:
#   http://www.accad.ohio-state.edu/~mlewis/VRML/Mocap
#
# Matthew Lewis, 10/12/2001
# mlewis@cgrg.ohio-state.edu
# http://www.accad.ohio-state.edu/~mlewis
#
########################################################

# usage
if ($#ARGV != 0) {
    print "usage: bvh2melskel in.bvh > out.mel\n";
    exit;
}

$bvhfn = $ARGV[0];  # bvh file name
open(BVH,"<$bvhfn");  # open bvh file

# make list of joint and segment names as read them
@jnts;   @segs;

# parse special first line "HIERARCHY"
$line = <BVH>;

# read ROOT line, get name, and pass to parse joint function
$line = <BVH>;
$line =~ /\s*ROOT\s+(\S+)/;
$jname = $1;
$jname = hanimName($jname);
# print "1st jname = $jname\n";

parseJoint("ROOT", $jname, 0,0,0, "");

print("\nselect -cl;\n");

close(BVH);

########################################
# read and output one joint
sub parseJoint {
    local $line;
    local $jtype = $_[0]; # joint type (ROOT or JOINT)
    local $jname = $_[1]; # joint name
    local $pjx = $_[2];   # parent joint global x position
    local $pjy = $_[3];   # parent joint global y position
    local $pjz = $_[4];   # parent joint global z position
    local $pjname = $_[5];     # parent joint's name

    local $jx = $pjx;     # joint global x position
    local $jy = $pjy;     # joint global y position
    local $jz = $pjz;     # joint global z position
    local $i;             # gen purpose index
    local $nc;            # num coords (child joints for seg making)
    local $sname;         # segment name (given a parent name)
    local $kidCoords = "";  # global coords of each child joint
    local $retCoords = "";  # return global coords to parent joint
    local $kidIndices = ""; # indices of line seg to child joints
    
    push(@jnts, $jname);
    
    $line = <BVH>;  # parse open curly bracket
#    print "open curly? : $line";
    
    # parse joint OFFSET
    $line = <BVH>;
    $line =~ /\s*OFFSET\s+(\S+)\s+(\S+)\s+(\S+)/;
    $ojx = $1;  $ojy = $2;  $ojz = $3;
#    print "Joint offset: ($ojx, $ojy, $ojz)\n";
    $jx += $ojx;  $jy += $ojy;  $jz += $ojz;  
#    print "Joint gpos: ($jx, $jy, $jz)\n";
    $retCoords = "$jx $jy $jz ";

    # find and store a segment name - child of this joint
    $sname = segName($jname);
    push(@segs, $sname);

    if($pjname ne "") {	print("select -r $pjname;\n"); }

#    print $psp."DEF hanim_".$jname." Joint {\n";  # start a new joint
#    print $psp."DEF hanim_".$jname." Transform {\n";  # start a new joint
    print "joint -n $jname -p $jx $jy $jz;\n";
#    print $psp."  name \"hanim_".$jname."\"\n";
    $pjname = $jname;
    
    # READ CHANNELS LINE
    # assume for now that for ROOT, it always looks like this:
    # CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
    # and for JOINT it always looks like this:
    # CHANNELS 3 Zrotation Xrotation Yrotation
    # note the wacky joint ordering!
    $line = <BVH>;  
    
    # NEXT is either a child joint, or an end site
    $line = <BVH>;  # "JOINT" or "End Site"
#    print "should be jnt or end site: $line";
    if($line =~ /\s*JOINT\s+(\S+)/) {
	$jname = $1;
	$jname = hanimName($jname);
#	print "Joint name: $jname\n";
	$kidCoords .= parseJoint("JOINT", $jname, $jx, $jy, $jz, $pjname);
	
    } elsif($line =~ /\s*End Site/) {
	$line = <BVH>;  # parse open curly bracket
	# parse End Site OFFSET
	$line = <BVH>;
	$line =~ /\s*OFFSET\s+(\S+)\s+(\S+)\s+(\S+)/;
	$ojx = $1;  $ojy = $2;  $ojz = $3;
#	print "End offset: ($ojx, $ojy, $ojz)\n";
#	$ejx = $jx + $ojx; $ejy = $jy + $ojy; $ejz = $jz + $ojz;
#	print "End gpos: ($ejx, $ejy, $ejz)\n";
	$line = <BVH>;  # parse close curly bracket
    }
    
    # end of joints?  or another child joint?
    while(!(($line = <BVH>) =~ /\s*\}/)) {  # while not close curly...
        $line =~ /\s*JOINT\s+(\S+)/;
	$jname = $1;
	$jname = hanimName($jname);
#	print "Joint name: $jname\n";
	$kidCoords .= parseJoint("JOINT", $jname, $pjx, $pjy, $pjz, $pjname);
    }
    
    # how many line segs should we draw?
    $nc = int(split(/ /,$kidCoords))/3;
    for($i=1; $i<=$nc; $i++) { $kidIndices .= "0 $i ";    }

    # create segment for this joint
    # currently using one line seg per child joint
#    print $psp."    DEF hanim_$sname Group {\n";
#    print $psp."      children [\n";
#    print $psp."        Transform {\n";
#    print $psp."          translation $jx $jy $jz\n";
#    print $psp."          scale 5 5 5\n";
#    print $psp."          children USE MARKER\n";
#    print $psp."        }\n";
#
#    if($kidCoords ne "") {
#	print $psp."        Shape {\n";
#	print $psp."          appearance Appearance {\n";
#	print $psp."            material Material { emissiveColor 0 0 0 }\n";
#	print $psp."          }\n";
#	print $psp."          geometry IndexedLineSet {\n";
#	print $psp."            coord Coordinate {\n";
#	print $psp."              point [ $jx $jy $jz $kidCoords ]\n";
#	#print $psp."              point [ $pjx $pjy $pjz $jx $jy $jz ]\n";
#	print $psp."            }\n";
#	print $psp."            coordIndex [ $kidIndices ]\n";
#	print $psp."          }\n";
#	print $psp."        }\n";
#    }
#
#    print $psp."      ]\n";  # close Segment's children
#    print $psp."    }\n";  # close Segment
#    print $psp."  ]\n";  # close Joint's children
#    print $psp."}\n";   # close Joint

    return $retCoords;
}

########################################
sub round() {
    local $v = $_[0];
    local $fv = int($v);
    if(($v-$fv)<0.5) { return $fv; } 
    else { return ceil($v); }
}

########################################
# multiply ry * rx * rz and return angle/axis rep of orientation
# these bvh files really seem to be doing yxz order...
sub zxy2aa()
{
    local $rz = $_[0];
    local $rx = $_[1];
    local $ry = $_[2];
    local $aa;           # angle axis orientation

    # build three quaternions from angle and axis
    $qz[2] = sin(radians($rz/2));   $qz[3] = cos(radians($rz/2));
    $qx[0] = sin(radians($rx/2));   $qx[3] = cos(radians($rx/2));
    $qy[1] = sin(radians($ry/2));   $qy[3] = cos(radians($ry/2));

    # multiply Yrot times Xrot
    $qyx[0] = $qx[3] * $qy[0] + $qx[0] * $qy[3] +
	$qx[1] * $qy[2] - $qx[2] * $qy[1];
    $qyx[1] = $qx[3] * $qy[1] + $qx[1] * $qy[3] +
	$qx[2] * $qy[0] - $qx[0] * $qy[2];
    $qyx[2] = $qx[3] * $qy[2] + $qx[2] * $qy[3] +
	$qx[0] * $qy[1] - $qx[1] * $qy[0];
    $qyx[3] = $qx[3] * $qy[3] - $qx[0] * $qy[0] -
	$qx[1] * $qy[1] - $qx[2] * $qy[2];
    $qlen = sqrt($qyx[0]*$qyx[0]+$qyx[1]*$qyx[1]+
		 $qyx[2]*$qyx[2]+$qyx[3]*$qyx[3]);
    $qyx[0]/=$qlen; $qyx[1]/=$qlen; $qyx[2]/=$qlen; $qyx[3]/=$qlen;

    # multiply YXrot times Zrot
    $qyxz[0] = $qz[3] * $qyx[0] + $qz[0] * $qyx[3] +
	$qz[1] * $qyx[2] - $qz[2] * $qyx[1];
    $qyxz[1] = $qz[3] * $qyx[1] + $qz[1] * $qyx[3] +
	$qz[2] * $qyx[0] - $qz[0] * $qyx[2];
    $qyxz[2] = $qz[3] * $qyx[2] + $qz[2] * $qyx[3] +
	$qz[0] * $qyx[1] - $qz[1] * $qyx[0];
    $qyxz[3] = $qz[3] * $qyx[3] - $qz[0] * $qyx[0] -
	$qz[1] * $qyx[1] - $qz[2] * $qyx[2];
    $qlen = sqrt($qyxz[0]*$qyxz[0]+$qyxz[1]*$qyxz[1]+
		 $qyxz[2]*$qyxz[2]+$qyxz[3]*$qyxz[3]);
    $qyxz[0]/=$qlen; $qyxz[1]/=$qlen; $qyxz[2]/=$qlen; $qyxz[3]/=$qlen;

    # extract axis and angle (in radians)
    $aa = "0 0 1 0";
    $len = sqrt($qyxz[0]*$qyxz[0]+$qyxz[1]*$qyxz[1]+$qyxz[2]*$qyxz[2]);
    if($len > 0.00001) {
	$ilen = 1/$len;
	$qyxz[0] *= $ilen;  $qyxz[1] *= $ilen;  $qyxz[2] *= $ilen;
	$qyxz[3] = 2 * acos($qyxz[3]);

	# trim precision
	$qyxz[0] = int($qyxz[0]*10000)/10000;
	$qyxz[1] = int($qyxz[1]*10000)/10000;
	$qyxz[2] = int($qyxz[2]*10000)/10000;
	$qyxz[3] = int($qyxz[3]*10000)/10000;

	$aa = "$qyxz[0] $qyxz[1] $qyxz[2] $qyxz[3]";
    }
    return $aa;
}

########################################
sub radians {
    local $d = $_[0];
    return $d * 0.017453293;
}

########################################
sub degrees {
    local $r = $_[0];
    return $r * 57.295779513;
}

########################################
sub acos { atan2( sqrt(1 - $_[0] * $_[0]), $_[0] ) }
sub min { ($_[0] < $_[1]) ? $_[0] : $_[1] }
sub max { ($_[0] > $_[1]) ? $_[0] : $_[1] }

########################################
sub segName {
    local $pname = $_[0];  # parent joint name
    local $sname = $pname;     # segment name

    if($pname eq "HumanoidRoot") { $sname = "sacrum"; }
    elsif($pname eq "vl5") { $sname = "l5"; }
    elsif($pname eq "vl1") { $sname = "l1"; }
    elsif($pname eq "vt6") { $sname = "t6"; }
    elsif($pname eq "vt1") { $sname = "t1"; }
    elsif($pname eq "skullbase") { $sname = "skull"; }
    elsif($pname eq "l_sternoclavicular") { $sname = "l_clavicle"; }
    elsif($pname eq "l_shoulder") { $sname = "l_upperarm"; }
    elsif($pname eq "l_elbow") { $sname = "l_forearm"; }
    elsif($pname eq "l_wrist") { $sname = "l_hand"; }
    elsif($pname eq "r_sternoclavicular") { $sname = "r_clavicle"; }
    elsif($pname eq "r_shoulder") { $sname = "r_upperarm"; }
    elsif($pname eq "r_elbow") { $sname = "r_forearm"; }
    elsif($pname eq "r_wrist") { $sname = "r_hand"; }
    elsif($pname eq "l_hip") { $sname = "l_thigh"; }
    elsif($pname eq "l_knee") { $sname = "l_calf"; }
    elsif($pname eq "l_ankle") { $sname = "l_hindfoot"; }
    elsif($pname eq "l_midtarsal") { $sname = "l_middistal"; }
    elsif($pname eq "r_hip") { $sname = "r_thigh"; }
    elsif($pname eq "r_knee") { $sname = "r_calf"; }
    elsif($pname eq "r_ankle") { $sname = "r_hindfoot"; }
    elsif($pname eq "r_midtarsal") { $sname = "r_middistal"; }
    return $sname;
}

########################################
sub hanimName {
    local $bname = $_[0];  # bvh name (local to accad?)
    local $sname = $bname;     # equiv hanim name

    if($bname eq "root") { $hname = "HumanoidRoot"; }
    elsif($bname eq "lowerback") { $hname = "vl5"; }
    elsif($bname eq "upperback") { $hname = "vl1"; }
    elsif($bname eq "thorax") { $hname = "vt6"; }
    elsif($bname eq "neck") { $hname = "vt1"; }
    elsif($bname eq "head") { $hname = "skullbase"; }
    elsif($bname eq "lshoulderjoint") { $hname = "l_sternoclavicular"; }
    elsif($bname eq "lhumerus") { $hname = "l_shoulder"; }
    elsif($bname eq "lradius") { $hname = "l_elbow"; }
    elsif($bname eq "lhand") { $hname = "l_wrist"; }
    elsif($bname eq "rshoulderjoint") { $hname = "r_sternoclavicular"; }
    elsif($bname eq "rhumerus") { $hname = "r_shoulder"; }
    elsif($bname eq "rradius") { $hname = "r_elbow"; }
    elsif($bname eq "rhand") { $hname = "r_wrist"; }
    elsif($bname eq "lfemur") { $hname = "l_hip"; }
    elsif($bname eq "ltibia") { $hname = "l_knee"; }
    elsif($bname eq "lfoot") { $hname = "l_ankle"; }
    elsif($bname eq "ltoes") { $hname = "l_midtarsal"; }
    elsif($bname eq "rfemur") { $hname = "r_hip"; }
    elsif($bname eq "rtibia") { $hname = "r_knee"; }
    elsif($bname eq "rfoot") { $hname = "r_ankle"; }
    elsif($bname eq "rtoes") { $hname = "r_midtarsal"; }
    return $hname;
}

########################################
########################################