Pose GetPoseLiftUpperArm(Pose pose, float distance)
{
var ikTarget = _ik.GetTarget(pose);
var baseLoc = _ik.BaseLocation;
var length = GetDistance(baseLoc.Y, baseLoc.Z, ikTarget.Result.Y, ikTarget.Result.Z);
//calculate rize in degrees (SSS triangle)
var upperArmOffset = (float)(Math.Acos((Math.Pow(length, 2) + Math.Pow(length, 2) - Math.Pow(distance, 2)) / (2 * length * length)) * (180 / Math.PI));
var newPose = pose.Copy();
newPose[1] -= upperArmOffset;
return(newPose);
}
/*
* Reads a DAZ Studio PZ2 file with a pose in it and produces
* a Vixen Pose object for this avatar's skeleton.
* @param posename file name containing pose
*/
public Pose ReadPose(System.String name)
{
ArrayList bones = new ArrayList();
ArrayList positions = new ArrayList();
int level = 0;
int numbones = 0;
Int32 numframes = 0;
bool parsing_motion = false;
System.String bonename = "";
Pose bindpose = BodyPoser.BindPose;
Pose pose = new Pose(BodyPoser);
float x, y, z;
pose.Copy(bindpose);
try
{
/*
* Parse the DAZ Studio pose file and extract the rotations
* for each bone into a hash table
*/
using (StreamReader posefile = new StreamReader("pose/" + name + ".bvh"))
{
System.String line;
char[] space = new char[] { ' ' };
while ((line = posefile.ReadLine().Trim()) != null)
{
string[] words = line.Split(space);
string opcode;
if (line == "")
{
continue;
}
/*
* Parsing motion for each frame.
* Each line in the file contains the root joint position and rotations for all joints.
*/
if (parsing_motion)
{
int nbones = 0;
if (words[0].StartsWith("Frame"))
{
continue;
}
x = float.Parse(words[0]); // root bone position
y = float.Parse(words[1]);
z = float.Parse(words[2]);
pose.SetPosition(new Vec3(x, y, z));
for (int i = 3; i < words.Length; i += 3)
{
bonename = bones[nbones] as System.String;
int boneindex = BodyPoser.GetBoneIndex(bonename);
++nbones;
if (boneindex >= 0)
{
Quat q, b;
z = float.Parse(words[i]); // Z, Y, X rotation angles
y = float.Parse(words[i + 1]);
x = float.Parse(words[i + 2]);
if ((x == 0) && (y == 0) && (z == 0))
{
continue;
}
if (true)
{
q = new Quat(Model.ZAXIS, z * (float)Math.PI / 180);
q *= new Quat(Model.YAXIS, x * (float)Math.PI / 180);
q *= new Quat(Model.XAXIS, y * (float)Math.PI / 180);
}
else
{
q = new Quat(Model.ZAXIS, y * (float)Math.PI / 180);
q *= new Quat(Model.YAXIS, x * (float)Math.PI / 180);
q *= new Quat(Model.XAXIS, z * (float)Math.PI / 180);
}
q.Normalize();
b = pose.GetLocalRotation(boneindex);
q *= b;
pose.SetLocalRotation(boneindex, q);
SharedWorld.Trace(bonename + " " + q.x + " " + q.y + " " + q.z + " " + q.w + "\n");
}
else
{
Canvas3D.LogError("Cannot find bone " + bonename + " in skeleton ");
}
}
break;
}
/*
* Parsing skeleton definition with joint names and positions.
*/
else
{
if (words.Length < 1) // has an argument?
{
continue;
}
opcode = words[0];
if ((opcode == "ROOT") || // found root bone?
(opcode == "JOINT")) // found any bone?
{
bonename = words[1]; // save the bone name
++numbones;
}
else if (opcode == "OFFSET") // bone position
{
float xpos = float.Parse(words[1]);
float ypos = float.Parse(words[2]);
float zpos = float.Parse(words[3]);
if (bonename.Length > 0) // save position for the bone
{
bones.Add(bonename);
positions.Add(new Vec3(xpos, ypos, zpos));
SharedWorld.Trace(bonename + " " + xpos + " " + ypos + " " + zpos + "\n");
}
bonename = "";
continue;
}
else if (opcode == "MOTION")
{
parsing_motion = true;
if (numbones != BodyPoser.NumBones)
{
Canvas3D.LogError(System.String.Format("Pose with {0} bones does not match skeleton with {1} bones", numbones, BodyPoser.NumBones));
}
}
}
}
}
return(pose);
}
catch (Exception ex)
{
Canvas3D.LogError(ex.Message + " Cannot read pose file " + name);
return(null);
}
}
