|
import mocap.figure.BoneGeom;
|
|
import javax.media.j3d.Transform3D;
|
import javax.vecmath.Vector3d;
|
import javax.vecmath.Vector2d;
|
import javax.vecmath.Quat4d;
|
import javax.vecmath.Matrix4d;
|
|
import com.jme.math.Quaternion;
|
|
import java.io.*;
|
|
//import com.bulletphysics.linearmath.QuaternionUtil;
|
|
|
/*
|
* To change this template, choose Tools | Templates
|
* and open the template in the editor.
|
*/
|
|
/**
|
*
|
* @author nbriere
|
*/
|
public class Mocap extends Object3D
|
{
|
static final long serialVersionUID = 7437391692559853707L;
|
|
void WriteTo(java.io.Writer writer) throws Exception
|
{
|
writer.write("MOTION\n");
|
int ff = firstframe;
|
int lf = lastframe;
|
if (lf == 0)
|
lf = bvh.animation.getNumFrames();
|
|
float[][] data = bvh.animation._data;
|
|
writer.write("Frames: " + (lf-ff));
|
writer.write("\nFrame Time: 0.00833333\n");
|
|
for (int f=ff; f<lf; f++)
|
{
|
for (int b=0; b<data.length; b++)
|
{
|
int dof = 3;
|
|
if (b == 0)
|
dof = 6;
|
|
for (int d=0; d<dof; d++)
|
{
|
float val = data[b][f*dof + d];
|
|
if (b != 0 || d >= 3)
|
val = (float)Math.toDegrees(val);
|
else
|
val /= bvh.animation.scale;
|
|
writer.write(Float.toString(val));
|
writer.write(" ");
|
}
|
}
|
|
writer.write("\n");
|
}
|
}
|
|
String fullname;
|
int beginframe, endframe;
|
|
// transient // aout 2013
|
mocap.reader.BVHReader.BVHResult bvh;
|
|
// transient // aout 2013
|
Object3D skeleton;
|
|
boolean smoothed;
|
|
//boolean touched;
|
|
int baseframe = 0; // playback frame
|
|
int frame = 0; // real frame after dynamic offset for crowds
|
|
int firstframe = 0; // for local playback and export
|
int lastframe = 0;
|
|
String wavname;
|
|
double myvolume = 0;
|
|
transient Wav wav;
|
|
double ground;
|
double pinx;
|
double pinz;
|
|
double targetx;
|
double targetz;
|
|
double goalx;
|
double goalz;
|
|
boolean followpath;
|
|
class FromTo
|
{
|
int inside;
|
|
cVector from;
|
cVector to;
|
|
FromTo(cVector f, cVector t)
|
{
|
from = f;
|
to = t;
|
}
|
}
|
|
class ScriptAction
|
{
|
int inside;
|
|
cVector from;
|
ScriptNode script;
|
// String filename;
|
String[] command;
|
int offset;
|
|
// ScriptAction(cVector f, ScriptNode s, String fn)
|
// {
|
// from = f;
|
// script = s;
|
// filename = fn;
|
// command = null;
|
// }
|
|
ScriptAction(cVector f, ScriptNode s, String[] cmd, int off)
|
{
|
from = f;
|
script = s;
|
// filename = null;
|
command = cmd;
|
offset = off;
|
}
|
}
|
|
transient java.util.Vector<FromTo> fromtos;
|
transient java.util.Vector<ScriptAction> scriptactions;
|
|
static cVector testvect = new cVector();
|
|
void ClearPath()
|
{
|
if (fromtos == null)
|
{
|
fromtos = new java.util.Vector<FromTo>();
|
}
|
else
|
fromtos.clear();
|
}
|
|
void ClearActions()
|
{
|
if (scriptactions == null)
|
{
|
scriptactions = new java.util.Vector<ScriptAction>();
|
}
|
else
|
scriptactions.clear();
|
}
|
|
static double EPSACTION = 1; // 0.075; // 0.1; // 0.075
|
|
void AddFromTo(cVector from, cVector to)
|
{
|
if (fromtos == null)
|
{
|
fromtos = new java.util.Vector<FromTo>();
|
}
|
|
for (int i=0; i<fromtos.size(); i++)
|
{
|
FromTo fromto = fromtos.get(i);
|
|
testvect.set(from);
|
testvect.sub(fromto.from);
|
|
if (testvect.dot(testvect) < EPSACTION)
|
{
|
// "from" already exists
|
return;
|
}
|
}
|
|
System.err.println("fromto " + from + " --> " + to);
|
fromtos.add(new FromTo(from,to));
|
}
|
|
// void AddAction(cVector from, ScriptNode to, String fn)
|
// {
|
// if (scriptactions == null)
|
// {
|
// scriptactions = new java.util.Vector<ScriptAction>();
|
// }
|
//
|
//// for (int i=0; i<scriptactions.size(); i++)
|
//// {
|
//// ScriptAction fromto = scriptactions.get(i);
|
////
|
//// testvect.set(from);
|
//// testvect.sub(fromto.from);
|
////
|
//// if (testvect.dot(testvect) < EPSACTION)
|
//// {
|
//// // "from" already exists
|
//// return;
|
//// }
|
//// }
|
//
|
// scriptactions.add(new ScriptAction(from,to,fn));
|
// }
|
|
void AddAction(cVector from, ScriptNode script, String[] cmd, int offset)
|
{
|
if (scriptactions == null)
|
{
|
scriptactions = new java.util.Vector<ScriptAction>();
|
}
|
|
// for (int i=0; i<scriptactions.size(); i++)
|
// {
|
// ScriptAction fromto = scriptactions.get(i);
|
//
|
// testvect.set(from);
|
// testvect.sub(fromto.from);
|
//
|
// if (testvect.dot(testvect) < EPSACTION)
|
// {
|
// // "from" already exists
|
// return;
|
// }
|
// }
|
|
System.err.print("add command " + from + " :");
|
for (int c=offset; c<cmd.length; c++)
|
System.err.print(" " + cmd[c]);
|
System.err.println();
|
scriptactions.add(new ScriptAction(from,script,cmd, offset));
|
}
|
|
void SetPath(Object3D path, ScriptNode script, boolean jump)
|
{
|
for (int i=0; i<path.size(); i++)
|
{
|
if (path.get(i).live)
|
{
|
AddAction(new cVector(path.get(i).toParent[3][0], path.get(i).toParent[3][1], path.get(i).toParent[3][2]),
|
script, path.get(i).name.split(" "), 0);
|
}
|
|
if (i == 0)
|
{
|
if (jump)
|
{
|
if (toParent == null)
|
{
|
toParent = LA.newMatrix();
|
fromParent = LA.newMatrix();
|
}
|
else
|
{
|
LA.matIdentity(toParent);
|
LA.matIdentity(fromParent);
|
}
|
|
if (get(0).toParent != null)
|
{
|
LA.matIdentity(get(0).toParent);
|
LA.matIdentity(get(0).fromParent);
|
}
|
|
toParent[3][0] = path.get(i).toParent[3][0];
|
toParent[3][1] = path.get(i).toParent[3][1];
|
toParent[3][2] = path.get(i).toParent[3][2];
|
fromParent[3][0] = path.get(i).fromParent[3][0];
|
fromParent[3][1] = path.get(i).fromParent[3][1];
|
fromParent[3][2] = path.get(i).fromParent[3][2];
|
}
|
else
|
{
|
// ????????????
|
LA.matConcat(get(0).toParent, toParent, tempmatrix);
|
AddFromTo(new cVector(tempmatrix[3][0], // toParent[3][0] + get(0).toParent[3][0],
|
tempmatrix[3][1], // toParent[3][1] + get(0).toParent[3][1],
|
tempmatrix[3][2]), // toParent[3][2] + get(0).toParent[3][2]),
|
new cVector(path.get(i).toParent[3][0],
|
path.get(i).toParent[3][1],
|
path.get(i).toParent[3][2]));
|
}
|
|
continue;
|
}
|
|
AddFromTo(new cVector(path.get(i-1).toParent[3][0],path.get(i-1).toParent[3][1],path.get(i-1).toParent[3][2]),
|
new cVector(path.get(i).toParent[3][0], path.get(i).toParent[3][1], path.get(i).toParent[3][2]));
|
}
|
|
get(0).link2master = true; // hip orientation
|
}
|
|
cVector GetGoal(cVector from)
|
{
|
if (fromtos == null)
|
{
|
fromtos = new java.util.Vector<FromTo>();
|
}
|
|
for (int i=0; i<fromtos.size(); i++)
|
{
|
FromTo fromto = fromtos.get(i);
|
|
testvect.set(from);
|
testvect.sub(fromto.from);
|
|
int factor = 1;
|
|
//if (CameraPane.FAST)
|
// factor = 10;
|
|
if (testvect.dot(testvect) < EPSACTION*factor) // 1)
|
{
|
if (fromto.inside > 0)
|
continue;
|
|
fromto.inside = 100; // true;
|
|
return fromto.to;
|
}
|
else
|
{
|
if (fromto.inside > 0)
|
fromto.inside--; // = false;
|
}
|
}
|
|
return null;
|
}
|
|
void CheckForAction(cVector from)
|
{
|
if (scriptactions == null)
|
{
|
scriptactions = new java.util.Vector<ScriptAction>();
|
}
|
|
ScriptNode script = null;
|
//String filename = null;
|
String[] command = null;
|
|
for (int i=0; i<scriptactions.size(); i++)
|
{
|
ScriptAction fromto = scriptactions.get(i);
|
|
testvect.set(from);
|
testvect.sub(fromto.from);
|
|
int factor = 1;
|
|
//if (CameraPane.FAST)
|
// factor = 10;
|
|
if (testvect.dot(testvect) < EPSACTION*factor) // 1)
|
{
|
if (fromto.inside > 0)
|
continue;
|
|
fromto.inside = 100; // true;
|
|
script = fromto.script;
|
//filename = fromto.filename;
|
command = fromto.command;
|
|
if (command != null)
|
{
|
//System.err.println("action command: " + command[6]);
|
System.err.print("run command " + fromto.from + " :");
|
for (int c=fromto.offset; c<command.length; c++)
|
System.err.print(" " + command[c]);
|
System.err.println();
|
script.ComputerCommand(command, fromto.offset);
|
}
|
}
|
else
|
{
|
if (fromto.inside > 0)
|
fromto.inside--; // = false;
|
}
|
}
|
|
if (script == null)
|
return;
|
|
// if (filename != null)
|
// {
|
// // script file
|
// script.stack++;
|
// System.err.println("action file: " + filename);
|
// try
|
// {
|
// script.reader = script.readers[script.stack] = new ScriptNode.ScriptReader(new BufferedReader(new FileReader(filename)), script.duration, script.linecount);
|
// script.duration = 0;
|
// script.linecount = 0;
|
// }
|
// catch(Exception e)
|
// {
|
// e.printStackTrace();
|
// System.err.println(" ******* ERROR IN ACTION COMMAND *******");
|
// }
|
// }
|
// else
|
// {
|
// assert(command != null);
|
//
|
// //script.ComputerCommand(command, 6);
|
// }
|
}
|
|
Mocap(String s)
|
{
|
super(s);
|
|
link2master = true;
|
}
|
|
// SET SUPPORT
|
void linkVerticesThis(Object3D other)
|
{
|
support = other;
|
}
|
|
void InitIndices(Object3D input, String dataname, mocap.figure.Bone[] bones)
|
{
|
// assert(input.Size() == 1);
|
input.material = null;
|
|
if (input.name != null)
|
{
|
for (int i=bones.length; --i>=0;)
|
{
|
if (/*input.*/dataname.equals(bones[i].getName()))
|
{
|
mocap.figure.Bone bone = bones[i];
|
// // skip first node: translation
|
Object3D b = input; //.get(0);
|
|
b._index = bone.getIndex();
|
|
b._name = bone.getName();
|
|
b._isBone = true; // !b._name.equals("null");
|
input.CreateMaterial();
|
|
// private Bone _parent;
|
|
b._isRoot = bone.getParent() == null;
|
|
//b._isHip = false;
|
//if (!b._isRoot)
|
// b._isHip = bone.getParent().getParent() == null;
|
|
|
// private Bone[] _children = new Bone[0];
|
// private Switch _textSwitch;
|
// transient TransformGroup _baseTransTG
|
// ,_transTG
|
// ,_baseRotTG
|
// ,_rotTG
|
// ,_invBaseRotTG
|
// ;
|
// Transform3D _t1 = new Transform3D();
|
// Transform3D _t2 = new Transform3D();
|
// Transform3D _transTF = new Transform3D();
|
// Transform3D _tworld = new Transform3D();
|
// Vector3d _trans = new Vector3d();
|
b._dof = bone._dof; // rotation order
|
b._numDOF = bone._numDOF;
|
//b._translationEnabled = true; // translation can be switched off
|
//b._rotationEnabled = true; // rotation can be switched off
|
b._rotationType = bone._rotationType; // Euler rotation type
|
//b._scale = bone._scale; // scale is applied to all translational components
|
break;
|
}
|
}
|
}
|
|
// mocap.figure.Bone[] children = bone.getChildren();
|
|
// assert(children.length == input.Size());
|
|
for (int i = 0; i < input.Size(); i++)
|
{
|
//mocap.figure.Bone subbone = children[i];
|
Object3D subb = input.get(i);
|
String dname = subb.name;
|
if (subb.name != null && subb.name.equals("neck"))
|
dname = "head";
|
|
InitIndices(subb, dname, bones);
|
}
|
}
|
|
int CollectBones(mocap.figure.Bone b, mocap.figure.Bone[] bones, int index)
|
{
|
bones[index++] = b;
|
|
mocap.figure.Bone[] children = b.getChildren();
|
|
for (int i = 0; i < children.length; i++)
|
{
|
mocap.figure.Bone bone = children[i];
|
index = CollectBones(bone, bones, index);
|
}
|
|
return index;
|
}
|
|
int NumBones(mocap.figure.Bone b)
|
{
|
int numbones = 1;
|
|
mocap.figure.Bone[] children = b.getChildren();
|
|
for (int i = 0; i < children.length; i++)
|
{
|
mocap.figure.Bone bone = children[i];
|
numbones += NumBones(bone);
|
}
|
|
return numbones;
|
}
|
|
// void SetPose(mocap.figure.Bone b, mocap.figure.AnimData data, int f)
|
// {
|
//// if (touched)
|
//// {
|
//// skeleton = null;
|
//// GetSkeleton();
|
//// touched = false;
|
//// }
|
//
|
// b.setPose(f, data.getBoneData(b.getIndex()), null);
|
//
|
// mocap.figure.Bone[] children = b.getChildren();
|
//
|
// for (int i = 0; i < children.length; i++)
|
// {
|
// mocap.figure.Bone bone = children[i];
|
// SetPose(bone, data, f);
|
// }
|
//
|
// }
|
|
void SetPose(Object3D input, mocap.figure.AnimData data, int f)
|
{
|
if (input._isBone)
|
{
|
// skip first node: translation
|
// Object3D b = input.get(0);
|
|
setPose(input, f, data.getBoneData(input._index), false); //, null);
|
}
|
|
//mocap.figure.Bone[] children = b.getChildren();
|
|
for (int i = 0; i < input.Size(); i++)
|
{
|
Object3D bone = input.get(i);
|
SetPose(bone, data, f);
|
}
|
|
}
|
|
//mocap.figure.Bone[] bones;
|
int numbones;
|
|
transient double groundx,groundz;
|
transient double hipx,hipz;
|
|
transient long groundid; // for exact foot contact
|
|
//transient boolean contactfound; // same as groundid == 0
|
|
transient int lastsoundtime;
|
|
// static int currentwave = 1;
|
|
// transient int mywave = 0;
|
|
void SetPosition(Object3D sourcenode)
|
{
|
//assert(CameraPane.drawMode == CameraPane.SHADOW);
|
|
Object3D transformnode = new Object3D();
|
//sourcenode.parent;
|
|
// may 2014
|
//LA.matIdentity(transformnode.toParent);
|
|
cVector floor = new cVector();
|
cVector centroid = new cVector();
|
cVector tmp = new cVector();
|
|
this.count++; // currently drawing itself
|
|
//boolean success = sourcenode.getFloor(floor, true);
|
long floorid = sourcenode.getFloor(floor); // , sourcenode.parent);
|
|
CameraPane.debugpoints[0].name = "Floor = " + floorid;
|
CameraPane.debugpoints[0].toParent[3][0] = floor.x;
|
CameraPane.debugpoints[0].toParent[3][1] = floor.y;
|
CameraPane.debugpoints[0].toParent[3][2] = floor.z;
|
|
if (floorid == 0) // !success)
|
new Exception().printStackTrace();
|
//LA.xformPos(floor, sourcenode.parent.toParent, floor);
|
|
// Object3D hip = get(0);
|
|
//success = sourcenode.getCentroid(centroid, true);
|
boolean success = sourcenode.getCentroid(centroid); //, sourcenode.parent);
|
|
this.count--; // currently drawing itself
|
|
CameraPane.debugpoints[7].name = "Centroid = " + groundid; // centroid;
|
CameraPane.debugpoints[7].toParent[3][0] = centroid.x;
|
CameraPane.debugpoints[7].toParent[3][1] = centroid.y;
|
CameraPane.debugpoints[7].toParent[3][2] = centroid.z;
|
|
if (!success)
|
new Exception().printStackTrace();
|
|
//System.err.println("this = " + sourcenode + ": floor = " + floor.y + ", ground = " + ground);
|
if (transformnode.toParent == null)
|
{
|
transformnode.toParent = LA.newMatrix();
|
transformnode.fromParent = LA.newMatrix();
|
}
|
|
// hum...
|
// LA.xformPos(centroid, sourcenode.parent.toParent, centroid);
|
//
|
// if (sourcenode.parent.parent != null)
|
// sourcenode.parent.parent.TransformToWorld(centroid, centroid);
|
// sourcenode.parent.TransformToWorld(centroid, centroid);
|
tmp.set(centroid);
|
|
// jan 2014 sourcenode.parent.TransformToWorld(tmp);
|
|
CameraPane.debugpoints[1].name = "C+";
|
CameraPane.debugpoints[1].toParent[3][0] = tmp.x;
|
CameraPane.debugpoints[1].toParent[3][1] = tmp.y;
|
CameraPane.debugpoints[1].toParent[3][2] = tmp.z;
|
|
// tmp.set(pinx,0,pinz);
|
if (pinx != -123456)
|
{
|
tmp.x = pinx;
|
}
|
if (ground != -123456)
|
{
|
tmp.y = ground;
|
}
|
if (pinz != -123456)
|
{
|
tmp.z = pinz;
|
}
|
|
CameraPane.debugpoints[2].name = "Pin";
|
CameraPane.debugpoints[2].toParent[3][0] = tmp.x;
|
CameraPane.debugpoints[2].toParent[3][1] = tmp.y;
|
CameraPane.debugpoints[2].toParent[3][2] = tmp.z;
|
|
//if (sourcenode.parent.parent != null)
|
// jan 2014 sourcenode.parent.TransformToLocal(tmp); //, tmp);
|
|
CameraPane.debugpoints[3].name = "C-";
|
CameraPane.debugpoints[3].toParent[3][0] = tmp.x;
|
CameraPane.debugpoints[3].toParent[3][1] = tmp.y;
|
CameraPane.debugpoints[3].toParent[3][2] = tmp.z;
|
|
// double posx = sourcenode.parent.toParent[3][0];
|
// double posz = sourcenode.parent.toParent[3][2];
|
|
double w = 0.00015;
|
|
//if (!sourcenode.link2master) // strong pin on floor
|
if (parent.link2master) // strong pin on floor
|
{
|
w = 0.005; // .001;
|
}
|
|
if (CameraPane.FAST || CameraPane.fullreset)
|
w = 1; // 0.05; // 1;
|
|
LA.matIdentity(tempmatrix);
|
|
if (pinx != -123456)
|
{
|
tempmatrix[3][0] = w * (tmp.x - centroid.x);
|
|
// sourcenode.parent.toParent[3][0] = (pinx - centroid.x)*w + posx*(1-w);
|
//sourcenode.parent.toParent[3][0] = pinx*w + centroid.x*(1-w);
|
// LA.xformPos(centroid, sourcenode.parent.toParent, tmp);
|
// groundx = (pinx + floor.x - tmp.x) * w + groundx * (1-w);
|
groundx += tempmatrix[3][0];
|
hipx += tempmatrix[3][0];
|
}
|
if (ground != -123456)
|
{
|
double w0 = w;
|
|
if (tmp.y - floor.y > 0.3)
|
// floor too far away, jump to it
|
w0 = 1;
|
|
tempmatrix[3][1] = Math.sqrt(Math.sqrt(w0)) * (tmp.y - floor.y);
|
}
|
if (pinz != -123456)
|
{
|
tempmatrix[3][2] = w * (tmp.z - centroid.z);
|
// sourcenode.parent.toParent[3][2] = (pinz - centroid.z)*w + posz*(1-w);
|
//sourcenode.parent.toParent[3][2] = pinz*w + centroid.z*(1-w);
|
// LA.xformPos(centroid, sourcenode.parent.toParent, tmp);
|
// groundz = (pinz + floor.z - tmp.z) * w + groundz * (1-w);
|
//groundz += (pinz - tmp.z) * w;
|
//groundz += sourcenode.parent.toParent[3][2] - posz;
|
groundz += tempmatrix[3][2];
|
hipz += tempmatrix[3][2];
|
}
|
|
// LA.matConcat(tempmatrix, sourcenode.parent.toParent, sourcenode.parent.toParent);
|
LA.matConcat(transformnode.toParent, tempmatrix, transformnode.toParent);
|
|
// sourcenode.parent.TransformToWorld(tmp, tmp);
|
|
double tempg = floor.y;
|
|
// jan 2014
|
// LA.xformPos(floor, sourcenode.parent.toParent, floor);
|
//
|
// CameraPane.debugpoints[4].name = "F+";
|
// CameraPane.debugpoints[4].toParent[3][0] = floor.x;
|
// CameraPane.debugpoints[4].toParent[3][1] = floor.y;
|
// CameraPane.debugpoints[4].toParent[3][2] = floor.z;
|
//
|
// if (sourcenode.parent.parent != null)
|
// sourcenode.parent.parent.TransformToWorld(floor); //, floor);
|
|
CameraPane.debugpoints[5].name = "F++";
|
CameraPane.debugpoints[5].toParent[3][0] = floor.x;
|
CameraPane.debugpoints[5].toParent[3][1] = floor.y;
|
CameraPane.debugpoints[5].toParent[3][2] = floor.z;
|
|
floor.y = tempg;
|
|
CameraPane.debugpoints[6].name = "F+-";
|
CameraPane.debugpoints[6].toParent[3][0] = floor.x;
|
CameraPane.debugpoints[6].toParent[3][1] = floor.y;
|
CameraPane.debugpoints[6].toParent[3][2] = floor.z;
|
|
if (pinx != -123456)
|
{
|
// LA.xformPos(centroid, tempmatrix, tmp);
|
// LA.xformPos(centroid, sourcenode.parent.toParent, tmp);
|
// groundx = (pinx + floor.x - tmp.x) * w + groundx * (1-w);
|
}
|
if (pinz != -123456)
|
{
|
// LA.xformPos(centroid, sourcenode.parent.toParent, tmp);
|
// groundz = (pinz + floor.z - tmp.z) * w + groundz * (1-w);
|
//groundz += (pinz - tmp.z) * w;
|
//groundz += sourcenode.parent.toParent[3][2] - posz;
|
}
|
|
boolean footcontact = CameraPane.FOOTCONTACT; // false;
|
|
if (footcontact && ground != -123456)
|
{
|
// foot contact
|
// double ox = groundx - floor.x;
|
// double oz = groundz - floor.z;
|
//
|
// double dist2 = ox*ox + oz*oz;
|
|
if (groundid != floorid) // !contactfound || dist2 > 0.0075) // .005) // .05)
|
{
|
groundx = floor.x; // + posx;
|
groundz = floor.z; // + posz;
|
|
groundid = floorid;
|
|
CameraPane.debugpoint.toParent[3][0] = groundx;
|
CameraPane.debugpoint.toParent[3][1] = floor.y;
|
CameraPane.debugpoint.toParent[3][2] = groundz;
|
|
if (true) // slow && stepout && onein)
|
{
|
// System.out.println("Play sound (" + dist2 + ") " + sourcenode);
|
|
// if (mywave == 0)
|
// {
|
// mywave = currentwave++;
|
// }
|
// sound
|
cVector eye = CameraPane.theRenderer.eyeCamera.location;
|
|
// if (sourcenode.parent.parent != null)
|
// sourcenode.parent.parent.TransformToWorld(floor, tmp);
|
// else
|
tmp.set(floor);
|
|
tmp.sub(eye);
|
|
if (groundid != 0 && wavname != null) // CameraPane.framecount - lastsoundtime > 30) // 30) // ~0.25 secs
|
{
|
double volume = 1 // (Math.random()+0.5)
|
/ Math.max(tmp.length2(), 1); // 0.2);
|
|
double dx = hipx - centroid.x;
|
double dz = hipz - centroid.z;
|
|
double hipmotion2 = Math.sqrt(dx*dx + dz*dz);
|
|
hipmotion2 *= 4;
|
|
if (hipmotion2 > 1)
|
{
|
// System.err.println("WARNING hipmotion = " + hipmotion2);
|
//new Exception().printStackTrace();
|
hipmotion2 = 1;
|
}
|
|
// System.out.print(" volume = " + volume);
|
// System.out.println("; hipmotion = " + hipmotion2);
|
|
volume *= hipmotion2;
|
|
if (wav == null)
|
wav = new Wav(wavname);
|
|
double usedvolume = 1;
|
|
if (myvolume != 0)
|
usedvolume = myvolume;
|
|
// System.out.println("play sound: " + (volume * usedvolume));
|
//GraphreeD.
|
wav.play(volume * usedvolume); //, mywave);
|
|
lastsoundtime = CameraPane.framecount;
|
}
|
// else
|
// System.out.println("skipped");
|
|
hipx = centroid.x;
|
hipz = centroid.z;
|
|
//stepout = false;
|
}
|
|
//contactfound = true;
|
}
|
|
// floor.x += posx;
|
// floor.z += posz;
|
|
// System.out.println("Allo " + sourcenode);
|
CameraPane.debugpoint2.toParent[3][0] = floor.x;// + posx;
|
CameraPane.debugpoint2.toParent[3][1] = ground; // floor.y;// + sourcenode.parent.toParent[3][1];
|
CameraPane.debugpoint2.toParent[3][2] = floor.z;// + posz;
|
|
|
if (false) // dist2 > 0.1)
|
{
|
groundx = floor.x;
|
groundz = floor.z;
|
}
|
|
double oldposx = transformnode.toParent[3][0];
|
double oldposy = transformnode.toParent[3][1];
|
double oldposz = transformnode.toParent[3][2];
|
|
double K = 1; // 0.25; // 1; // 0.05;
|
|
// LA.matIdentity(tempmatrix);
|
|
// tempmatrix[3][0] = K * (groundx - floor.x);
|
// tempmatrix[3][1] = K * (ground - floor.y);
|
// tempmatrix[3][2] = K * (groundz - floor.z);
|
//
|
// LA.matConcat(tempmatrix, sourcenode.parent.toParent, sourcenode.parent.toParent);
|
|
// sourcenode.parent.toParent[3][0] += groundx - floor.x;
|
// sourcenode.parent.toParent[3][1] = ground - floor.y;
|
// sourcenode.parent.toParent[3][2] += groundz - floor.z;
|
// sourcenode.parent.toParent[3][1] = K * (ground - floor.y) + (1 - K) * oldposy;
|
if (footcontact && !sourcenode.link2master) // strong pin on floor
|
{
|
v0.x = groundx - floor.x;
|
v0.y = 0; // groundx - floor.x;
|
v0.z = groundz - floor.z;
|
|
double distance = Math.sqrt(v0.dot(v0));
|
double maxdist = 0.004;
|
|
if (distance > maxdist)
|
{
|
// probably wrong foot
|
K = 0; // maxdist/distance;
|
}
|
//K = 1;
|
transformnode.toParent[3][0] = K * (oldposx + v0.x) + (1 - K) * oldposx;
|
transformnode.toParent[3][2] = K * (oldposz + v0.z) + (1 - K) * oldposz;
|
}
|
}
|
|
LA.matInvert(transformnode.toParent, transformnode.fromParent);
|
|
if (transformnode != sourcenode.parent)
|
{
|
//LA.matConcat(transformnode.toParent, sourcenode.toParent, sourcenode.toParent);
|
// ?????????
|
sourcenode = this; // may 2014
|
LA.matConcat(sourcenode.toParent, transformnode.toParent, sourcenode.toParent);
|
LA.matInvert(sourcenode.toParent, sourcenode.fromParent);
|
}
|
}
|
|
void SetRotation(Object3D sourcenode)
|
{
|
_t1.setIdentity();
|
|
Object3D rotsourcenode = sourcenode; // get(0).get(0);
|
|
// Object3D tempobj = new Object3D();
|
// LA.matCopy(rotsourcenode.toParent, tempobj.toParent);
|
|
// cJ3D.ResetTransform(get(0).get(0), _t1);
|
|
cVector src = new cVector();
|
cVector dst = new cVector();
|
cVector orig = new cVector();
|
cVector test = new cVector();
|
//cVector euler = new cVector();
|
|
cVector floor = new cVector();
|
cVector centroid = new cVector();
|
|
this.count++; // currently drawing itself
|
|
//boolean success = sourcenode.getFloor(floor, true);
|
long floorid = sourcenode.getFloor(floor); // , sourcenode.parent);
|
assert(floorid != 0);
|
boolean success = sourcenode.getCentroid(centroid); // , sourcenode.parent);
|
assert(success);
|
|
this.count--; // currently drawing itself
|
|
centroid.y = floor.y; // 0;
|
|
cVector goal = GetGoal(centroid);
|
|
if (goal != null)
|
{
|
System.err.println("GOAL change: " + goal + " (was " + goalx + ", " + goalz + ")");
|
//sourcenode.parent.parent.Dump();
|
goalx = goal.x;
|
goalz = goal.z;
|
targetx = targetz = 0;
|
|
followpath = true;
|
}
|
else
|
{
|
// if (followpath)
|
// {
|
// goalx = goalz = 0;
|
// followpath = false;
|
// }
|
}
|
|
sourcenode.TransformToLocal(floor);
|
// sourcenode.parent.TransformToLocal(floor);
|
|
// dst.x = 0;
|
// dst.y = 0;
|
// dst.z = -1;
|
|
Object3D speakernode = null;
|
|
if (ScriptNode.speaker != null)
|
{
|
speakernode = ScriptNode.speaker.GetFileRoot();
|
|
boolean uselast = false;
|
|
if (speakernode == sourcenode && ScriptNode.lastspeaker != null)
|
{
|
speakernode = ScriptNode.lastspeaker.GetFileRoot();
|
uselast = true;
|
}
|
|
//boolean
|
success = speakernode.getCentroid(dst, true);
|
|
if (!success)
|
new Exception().printStackTrace();
|
|
speakernode.parent.TransformToWorld(dst); //, dst); // aout 2013
|
sourcenode.parent.TransformToLocal(dst); //, dst);
|
// LA.xformPos(dst, speakernode.parent.toParent, dst);
|
//
|
// if (speakernode.parent.parent != null) // aout 2013
|
// {
|
// speakernode.parent.parent.TransformToWorld(dst, dst);
|
//// LA.xformPos(dst, speakernode.parent.parent.toParent, dst);
|
// }
|
if (false) // !uselast)
|
{
|
CameraPane.debugpoint.toParent[3][0] = dst.x;
|
CameraPane.debugpoint.toParent[3][1] = dst.y;
|
CameraPane.debugpoint.toParent[3][2] = dst.z;
|
}
|
}
|
|
if (goalx != 0 || goalz != 0) // TODO
|
{
|
// overwrite speaker orientation
|
dst.set(goalx,0,goalz);
|
sourcenode.parent.
|
TransformToLocal(dst);
|
}
|
|
//LA.xformDir(temp, rot.toParent, temp);
|
// LA.xformPos(dst, fromParent, dst);
|
|
// src.x = 0;
|
// src.y = 0;
|
// src.z = 0;
|
|
src.x = 0;
|
src.y = 0;
|
src.z = 0;
|
LA.xformPos(src, get(0).toParent, src);
|
LA.xformPos(src, toParent, src);
|
// LA.xformPos(src, rotsourcenode.toParent, src);
|
|
orig.set(src);
|
|
// rotsourcenode.TransformToWorld(orig);
|
|
if (rotsourcenode != this)
|
LA.xformPos(src, rotsourcenode.toParent, src);
|
|
// CameraPane.debugpoints[4].name = "ORIGIN";
|
// CameraPane.debugpoints[4].toParent[3][0] = orig.x;
|
// CameraPane.debugpoints[4].toParent[3][1] = orig.y;
|
// CameraPane.debugpoints[4].toParent[3][2] = orig.z;
|
|
// orig.set(src);
|
|
if (false) // aout 2013 rotsourcenode.parent != null)
|
{
|
LA.xformPos(src, rotsourcenode.parent.toParent, src);
|
if (rotsourcenode.parent.parent != null) // aout 2013
|
{
|
rotsourcenode.parent.parent.TransformToWorld(src); //, src);
|
//LA.xformPos(src, rot.parent.parent.toParent, src);
|
}
|
}
|
else
|
{
|
// new Exception().printStackTrace();
|
}
|
|
// mars 2014
|
if (false) // goalx != 0 || goalz != 0)
|
{
|
targetx = dst.x - src.x;
|
targetz = dst.z - src.z;
|
|
if (Math.abs(targetx) > 0.1 || Math.abs(targetz) > 0.1)
|
{
|
// far enough from goal. keep the goal position.
|
targetx = 0;
|
targetz = 0;
|
}
|
else
|
{
|
// too close to goal. switch to target direction instead.
|
goalx = 0;
|
goalz = 0;
|
}
|
}
|
|
//sourcenode.getCentroid(src, true);
|
|
if (speakernode == sourcenode && goalx == 0 && goalz == 0)
|
{
|
if (ScriptNode.lastspeaker != null)
|
new Exception().printStackTrace();
|
return;
|
}
|
|
// LA.xformPos(src, fromParent, src);
|
|
if ((ScriptNode.speaker != null && CameraPane.SPEAKERMOCAP || goalx != 0 || goalz != 0) && targetx == 0 && targetz == 0)
|
dst.sub(src);
|
else
|
// vector mode
|
{
|
dst.x = targetx;
|
dst.z = targetz;
|
|
// TEST TARGET
|
// dst.x = CameraPane.selectedpoint.toParent[3][0];
|
// dst.y = CameraPane.selectedpoint.toParent[3][1];
|
// dst.z = CameraPane.selectedpoint.toParent[3][2];
|
}
|
|
if (dst.x == 0 && dst.z == 0)
|
{
|
return;
|
}
|
|
double angleY = Math.atan2(dst.x, dst.z);
|
|
// src.x = 0;
|
// src.y = 1;
|
// src.z = 0;
|
// LA.xformDir(src, rot.toParent, src);
|
//
|
// double angleX = Math.atan2(-src.x, src.y);
|
|
if (rotsourcenode.toParent == null)
|
{
|
rotsourcenode.toParent = LA.newMatrix();
|
rotsourcenode.fromParent = LA.newMatrix();
|
}
|
|
double tx = rotsourcenode.toParent[3][0];
|
double ty = rotsourcenode.toParent[3][1];
|
double tz = rotsourcenode.toParent[3][2];
|
|
// dst.x = 0;
|
// dst.y = 0;
|
// dst.z = 0;
|
// LA.xformPos(dst, get(0).get(0).get(0).get(0).get(0).get(0).get(0).toParent, dst);
|
// LA.xformPos(dst, get(0).get(0).get(0).get(0).get(0).get(0).toParent, dst);
|
// LA.xformPos(dst, get(0).get(0).get(0).get(0).get(0).toParent, dst);
|
// LA.xformPos(dst, get(0).get(0).get(0).get(0).toParent, dst);
|
// LA.xformPos(dst, get(0).get(0).get(0).toParent, dst);
|
// LA.xformPos(dst, get(0).get(0).toParent, dst);
|
// LA.xformPos(dst, get(0).toParent, dst); // hip
|
// LA.xformPos(dst, toParent, dst);
|
|
dst.set(floor); // rotation center
|
|
test.set(dst);
|
|
rotsourcenode.TransformToWorld(test);
|
|
CameraPane.debugpoints[4].name = "ROT = "; // + test;
|
CameraPane.debugpoints[4].toParent[3][0] = test.x;
|
CameraPane.debugpoints[4].toParent[3][1] = test.y;
|
CameraPane.debugpoints[4].toParent[3][2] = test.z;
|
// LA.xformPos(src, rot.toParent, src);
|
|
// tx = src.x;
|
// ty = src.y;
|
// tz = src.z;
|
|
// assert(tx == 0);
|
// assert(ty == 0);
|
// assert(tz == 0);
|
|
// _trans.x = src.x;
|
// _trans.y = src.y;
|
// _trans.z = src.z;
|
//
|
// _t2.setIdentity();
|
// _t2.setTranslation(_trans);
|
//
|
// _t1.mul(_t2);
|
|
src.x = 1;
|
src.y = 0;
|
src.z = 0;
|
LA.xformDir(src, get(0).get(0).toParent, src);
|
// LA.xformDir(src, get(0).toParent, src);
|
// LA.xformDir(src, toParent, src);
|
|
double angleDY = Math.atan2(-src.z, src.x);
|
|
src.x = 1;
|
src.y = 0;
|
src.z = 0;
|
LA.xformDir(src, toParent, src);
|
|
double angleRY = Math.atan2(-src.z, src.x); // 0
|
|
double targetangle = angleY - (angleRY + angleDY);
|
|
src.x = 1;
|
src.y = 0;
|
src.z = 0;
|
LA.xformDir(src, rotsourcenode.toParent, src);
|
|
double currentangle = Math.atan2(-src.z, src.x);
|
|
if (ScriptNode.speaker == null && dst.x == 0 && dst.z == 0 || postponeangle != 0)
|
targetangle = currentangle;
|
|
_t2.setIdentity();
|
|
// while (targetangle < -Math.PI)
|
// targetangle += 2*Math.PI;
|
// while (currentangle < -Math.PI)
|
// currentangle += 2*Math.PI;
|
// while (targetangle > Math.PI)
|
// targetangle -= 2*Math.PI;
|
// while (currentangle > Math.PI)
|
// currentangle -= 2*Math.PI;
|
|
double sinc = Math.sin(currentangle/2);
|
double sint = Math.sin(targetangle/2);
|
double cosc = Math.cos(currentangle/2);
|
double cost = Math.cos(targetangle/2);
|
|
double K = 0.005; // TARGET ROTATION SPEED
|
|
// if (sourcenode.link2master)
|
// if (goalx != 0 || goalz != 0)
|
// K = 0.02; // .0625;
|
if (parent.link2master) // strong pin on floor
|
{
|
K = 0.05; // 0.02;
|
}
|
|
if (CameraPane.FAST)
|
K = 1; // 0.25; // 1; // *= 16;
|
|
double finalangle = Math.atan2(sinc*(1-K) + sint*K, cosc*(1-K) + cost*K);
|
|
// _t2.rotY(finalangle*2);
|
|
if (postponeangle != 0)
|
{
|
//System.err.println("sourcenode = " + sourcenode);
|
//System.err.println("currentangle = " + currentangle);
|
//System.err.println("targetangle = " + targetangle);
|
//System.err.println("postponeangle = " + postponeangle);
|
//_t2.rotY(CurveAngle(currentangle, targetangle, K));
|
_t2.rotY(targetangle); // + postponeangle);
|
// _t2.rotY(postponeangle);
|
postponeangle = 0;
|
}
|
else
|
{
|
double angle = CurveAngle(currentangle, targetangle, K);
|
//System.err.println("angle = " + angle);
|
_t2.rotY(angle);
|
}
|
|
// _t2.rotY(currentangle*K + targetangle*(1-K));
|
// _t2.rotX(angleX);
|
|
orig.set(dst);
|
|
if (true) // false)
|
{
|
src.set(dst);
|
|
LA.xformPos(src, rotsourcenode.toParent, src);
|
|
// LA.matCopy(rotsourcenode.toParent, matrix);
|
|
test.set(src);
|
|
cJ3D.ResetTransform(rotsourcenode, _t2);
|
|
// LA.matInvert(rotsourcenode.toParent, matrix2);
|
// LA.matConcat(matrix, matrix2, matrix);
|
|
LA.xformPos(dst, rotsourcenode.toParent, dst);
|
|
tx = orig.x; // rotsourcenode.toParent[3][0];
|
// ty = rotsourcenode.toParent[3][1];
|
tz = orig.z; // rotsourcenode.toParent[3][2];
|
|
//if (!sourcenode.link2master) // pin on floor
|
{
|
// mar 2013
|
tx = src.x - dst.x; //
|
ty = src.y - dst.y;
|
tz = src.z - dst.z; //
|
}
|
//else
|
{
|
// tx -= dst.x;
|
// ty -= dst.y;
|
// tz -= dst.z;
|
}
|
|
// javax.vecmath.Matrix4d m = new javax.vecmath.Matrix4d();
|
//
|
// m.m00 = rot.toParent[0][0];
|
// m.m10 = rot.toParent[0][1];
|
// m.m20 = rot.toParent[0][2];
|
// m.m30 = rot.toParent[0][3];
|
// m.m01 = rot.toParent[1][0];
|
// m.m11 = rot.toParent[1][1];
|
// m.m21 = rot.toParent[1][2];
|
// m.m31 = rot.toParent[1][3];
|
// m.m02 = rot.toParent[2][0];
|
// m.m12 = rot.toParent[2][1];
|
// m.m22 = rot.toParent[2][2];
|
// m.m32 = rot.toParent[2][3];
|
// m.m03 = rot.toParent[3][0];
|
// m.m13 = rot.toParent[3][1];
|
// m.m23 = rot.toParent[3][2];
|
// m.m33 = rot.toParent[3][3];
|
//
|
// _t1.set(m);
|
// _t1.mul(_t2);
|
//
|
// _t1.get(m);
|
//
|
// tx = m.m03;
|
// ty = m.m13;
|
// tz = m.m23;
|
//
|
// _t2.setIdentity();
|
// _t2.rotY(angleY - (angleRY + angleDY));
|
|
// _trans.x = -src.x;
|
// _trans.y = -src.y;
|
// _trans.z = -src.z;
|
//
|
// _t2.setIdentity();
|
// _t2.setTranslation(_trans);
|
//
|
// _t1.mul(_t2);
|
|
//matrixToEulerXYZ(rot.toParent, euler);
|
|
// cJ3D.ResetTransform(rot, _t2);
|
|
//
|
// LA.matIdentity(matrix);
|
// LA.matTranslate(matrix, -tx, 0, -tz);
|
//
|
// LA.matConcat(rotsourcenode.toParent, matrix, rotsourcenode.toParent);
|
//
|
// LA.matIdentity(matrix);
|
// LA.matTranslate(matrix, tx, 0, tz);
|
//
|
// LA.matConcat(matrix, rotsourcenode.toParent, rotsourcenode.toParent);
|
|
//// jan 2014
|
//// rotsourcenode.toParent[3][0] = tx;
|
// rotsourcenode.toParent[3][1] = ty;
|
//// rotsourcenode.toParent[3][2] = tz;
|
|
LA.matIdentity(matrix);
|
LA.matTranslate(matrix, tx, ty, tz);
|
// ?????????
|
LA.matConcat(rotsourcenode.toParent, matrix, rotsourcenode.toParent);
|
// LA.matConcat(matrix, rotsourcenode.toParent, rotsourcenode.toParent);
|
//
|
LA.xformPos(orig, rotsourcenode.toParent, orig);
|
|
if (!orig.toString().equals(test.toString()))
|
new Exception().printStackTrace();
|
|
LA.matInvert(rotsourcenode.toParent, rotsourcenode.fromParent);
|
|
//LA.matConcat(transformnode.toParent, sourcenode.toParent, sourcenode.toParent);
|
// ?????????
|
if (toParent == null)
|
{
|
toParent = LA.newMatrix();
|
fromParent = LA.newMatrix();
|
}
|
LA.matConcat(toParent, rotsourcenode.toParent, toParent);
|
LA.matInvert(toParent, fromParent);
|
|
LA.matIdentity(rotsourcenode.toParent);
|
LA.matIdentity(rotsourcenode.fromParent);
|
}
|
else
|
{
|
if (transformnode == null)
|
{
|
transformnode = new Object3D();
|
}
|
|
cJ3D.ResetTransform(transformnode, _t2);
|
|
tx = orig.x; // rotsourcenode.toParent[3][0];
|
// ty = rotsourcenode.toParent[3][1];
|
tz = orig.z; // rotsourcenode.toParent[3][2];
|
|
LA.matIdentity(matrix);
|
LA.matTranslate(matrix, -tx, 0, -tz);
|
|
LA.matConcat(rotsourcenode.parent.toParent, matrix, rotsourcenode.parent.toParent);
|
LA.matConcat(rotsourcenode.parent.toParent, transformnode.toParent, rotsourcenode.parent.toParent);
|
|
LA.matIdentity(matrix);
|
LA.matTranslate(matrix, tx, 0, tz);
|
|
LA.matConcat(rotsourcenode.parent.toParent, matrix, rotsourcenode.parent.toParent);
|
|
LA.matInvert(rotsourcenode.parent.toParent, rotsourcenode.parent.fromParent);
|
}
|
}
|
|
Object3D GetSkeleton()
|
{
|
// if (bvh.skeleton == null)
|
// {
|
// bvh.reader._indexCounter = 0;
|
//
|
// bvh.skeleton = bvh.reader.processJoint(bvh.joints, null, bvh.joints.findMax(0) / 15, false);
|
//
|
// touched = true;
|
// }
|
|
try
|
{
|
if (skeleton == null) // size() == 0)
|
{
|
// if (bvh == null)
|
// ReadBVH();
|
|
assert (bvh.skeleton == null);
|
//if (bvh.skeleton == null)
|
{
|
bvh.reader._indexCounter = 0;
|
|
bvh.skeleton = bvh.reader.processJoint(bvh.joints, null, bvh.joints.findMax(0) / 15, true); // false);
|
|
// touched = true;
|
}
|
|
//addChild(new cJ3D().Read(bvh.skeleton.getBaseTransTG()));
|
skeleton = new cJ3D().Read(bvh.skeleton.getBaseTransTG());
|
|
mocap.figure.Bone[] bones = new mocap.figure.Bone[numbones = NumBones(bvh.skeleton)];
|
|
int index = CollectBones(bvh.skeleton, bones, 0);
|
|
assert(index == bones.length);
|
|
InitIndices(skeleton, skeleton.name, bones);
|
|
skeleton.parent = this; // dec 2012
|
|
// // aout 2013
|
// for (int i=8; --i>=0;)
|
// SmoothAnimData(); // much reduces shakiness
|
}
|
|
if (!smoothed)
|
for (int i=4; --i>=0;)
|
SmoothAnimData(); // much reduces shakiness
|
|
smoothed = true;
|
}
|
catch (Exception e)
|
{
|
e.printStackTrace();
|
}
|
|
return skeleton;
|
}
|
|
// void setPose(int f)
|
// {
|
// GetSkeleton();
|
//
|
// //bvh.skeleton.setPose(f, bvh.animation.getBoneData(bvh.skeleton.getIndex()), null);
|
// SetPose(bvh.skeleton, bvh.animation, f);
|
//
|
// //new cJ3D().ResetTransform(get(0), bvh.skeleton.getBaseTransTG());
|
// new cJ3D().ResetTransform(GetSkeleton(), bvh.skeleton.getBaseTransTG());
|
// }
|
|
static Vector2d fromVector = new Vector2d();
|
static Vector2d toVector = new Vector2d();
|
static Vector2d vec = new Vector2d();
|
|
static double CurveAngle(double from, double to, double step)
|
{
|
if (step == 0) return from;
|
if (from == to || step == 1) return to;
|
|
fromVector.x = LA.cos(from);
|
fromVector.y = LA.sin(from);
|
toVector.x = LA.cos(to);
|
toVector.y = LA.sin(to);
|
|
Vector2d currentVector = Slerp(fromVector, toVector, step);
|
|
return Math.atan2(currentVector.y, currentVector.x);
|
}
|
|
public static Vector2d Slerp(Vector2d from, Vector2d to, double step)
|
{
|
double cosa = from.dot(to);
|
if (cosa >= 1) return to;
|
|
double theta = Math.acos(cosa);
|
if (theta == 0) return to;
|
|
double sinTheta = LA.sin(theta);
|
if (sinTheta == 0) return to;
|
|
double A = LA.sin((1 - step) * theta) / sinTheta;
|
double B = LA.sin(step * theta) / sinTheta;
|
|
// return A * from + B * to;
|
vec.x = A*from.x + B*to.x;
|
vec.y = A*from.y + B*to.y;
|
|
return vec;
|
}
|
|
double postponeangle;
|
|
void setPose(int f)
|
{
|
// System.err.println("setPose Mocap frame # " + frame);
|
// System.err.println("setPose Mocap baseframe # " + baseframe);
|
// System.err.println("setPose Mocap new frame # " + f);
|
// baseframe = // !!
|
frame = f;
|
|
if (!live) // aout 2013
|
return;
|
|
//bvh.skeleton.setPose(f, bvh.animation.getBoneData(bvh.skeleton.getIndex()), null);
|
try
|
{
|
SetPose(GetSkeleton(), bvh.animation, f);
|
}
|
catch (Exception e)
|
{
|
e.printStackTrace();
|
}
|
|
Object3D sourcenode = GetFileRoot();
|
|
if (sourcenode.parent == null && sourcenode.fileparent == null)
|
{
|
// no GFD
|
return;
|
}
|
|
if (get(0).link2master && !firsttimeafterload && frame > 0)
|
{
|
SetRotation(sourcenode);
|
}
|
|
if (sourcenode != null && sourcenode.parent != null &&
|
sourcenode.parent.link2master && !firsttimeafterload && frame > 0)
|
{
|
SetPosition(sourcenode);
|
}
|
|
// Check for action
|
this.count++; // currently drawing itself
|
|
cVector centroid = new cVector();
|
cVector floor = new cVector();
|
|
boolean success = sourcenode.getCentroid(centroid);
|
if (!success)
|
{
|
//assert(success);
|
//new Exception().printStackTrace();
|
System.out.println("No centroid for " + sourcenode);
|
}
|
|
long floorid = sourcenode.getFloor(floor);
|
//assert(floorid != 0);
|
if (floorid == 0)
|
{
|
//new Exception().printStackTrace();
|
System.out.println("No floor for " + sourcenode);
|
}
|
|
this.count--; // currently drawing itself
|
|
centroid.y = floor.y;
|
|
CheckForAction(centroid);
|
|
//new cJ3D().ResetTransform(get(0), bvh.skeleton.getBaseTransTG());
|
// new cJ3D().ResetTransform(GetSkeleton(), bvh.skeleton.getBaseTransTG());
|
}
|
|
|
/**
|
* Sets this bone/joint to the translation/rotation of the given frame.
|
* Called in the animation loop.
|
*
|
* @param frame
|
* Frame number
|
*/
|
// public void setPose(int frame, float[] data, Point3d offsetTrans)
|
// {
|
// // System.out.println("setPose " + frame);
|
// // _lastFrame = frame;
|
// // do something if joint has at least one DOF
|
// if (_numDOF > 0)
|
// {
|
// int offset = frame * _numDOF;
|
// if (offset + _numDOF - 1 < data.length)
|
// {
|
// boolean hasTranslation = false;
|
// if (_parent == null)
|
// {
|
// _trans.set(0, 0, 0);
|
// }
|
// _t1.setIdentity();
|
//
|
// // go through DOF in the specified order
|
// for (int i = 0; i < _numDOF; i++)
|
// {
|
// boolean isRotation = false;
|
// switch (_dof[i])
|
// {
|
// case RX:
|
// _t2.rotX(data[offset + i]);
|
// isRotation = true;
|
// break;
|
// case RY:
|
// _t2.rotY(data[offset + i]);
|
// isRotation = true;
|
// break;
|
// case RZ:
|
// _t2.rotZ(data[offset + i]);
|
// isRotation = true;
|
// break;
|
// case TX:
|
// _trans.x += _scale * data[offset + i];
|
// hasTranslation = true;
|
// isRotation = false;
|
// break;
|
// case TY:
|
// _trans.y += _scale * data[offset + i];
|
// hasTranslation = true;
|
// isRotation = false;
|
// break;
|
// case TZ:
|
// _trans.z += _scale * data[offset + i];
|
// hasTranslation = true;
|
// isRotation = false;
|
// break;
|
// }
|
//
|
// // multiply rotations
|
// if (isRotation)
|
// {
|
// // fixed axes: multiply from left
|
// if (_rotationType == FIXED_AXES)
|
// {
|
// if (i > 0)
|
// {
|
// _t2.mul(_t1);
|
// }
|
// _t1 = new Transform3D(_t2);
|
// } else
|
// {
|
// // moving axes: multiply from right
|
// _t1.mul(_t2);
|
// _t2.setIdentity();
|
// }
|
// }
|
// }
|
//
|
// // set rotation
|
// if (_rotationEnabled)
|
// {
|
// _rotTG.setTransform(_t1);
|
// }
|
//
|
// // add offset to translation of root
|
// if (offsetTrans != null && _parent == null)
|
// {
|
// _trans.add(offsetTrans);
|
// hasTranslation = true;
|
// }
|
//
|
// // set translation
|
// // (currently only possible for root bone! see mocapdata.com data)
|
// if (_parent == null)
|
// {
|
// if (hasTranslation && _translationEnabled)
|
// {
|
// _transTF.setIdentity();
|
// _transTF.setTranslation(_trans);
|
// _transTG.setTransform(_transTF);
|
// }
|
// }
|
// }
|
// }
|
// }
|
|
private static double getMatrixElem(double[][] mat, int index) {
|
int i = index % 3;
|
int j = index / 3;
|
return mat[j][i];
|
}
|
|
/**
|
* MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
|
*/
|
private static boolean matrixToEulerXYZ(double[][] mat, cVector xyz) {
|
// // rot = cy*cz -cy*sz sy
|
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
|
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
|
//
|
|
if (getMatrixElem(mat, 2) < 1.0f) {
|
if (getMatrixElem(mat, 2) > -1.0f) {
|
xyz.x = (float) Math.atan2(-getMatrixElem(mat, 5), getMatrixElem(mat, 8));
|
xyz.y = (float) Math.asin(getMatrixElem(mat, 2));
|
xyz.z = (float) Math.atan2(-getMatrixElem(mat, 1), getMatrixElem(mat, 0));
|
return true;
|
}
|
else {
|
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
|
xyz.x = -(float) Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4));
|
xyz.y = -Math.PI/2;
|
xyz.z = 0.0f;
|
return false;
|
}
|
}
|
else {
|
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
|
xyz.x = (float) Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4));
|
xyz.y = Math.PI/2;
|
xyz.z = 0.0f;
|
}
|
|
return false;
|
}
|
|
static
|
//transient
|
Transform3D _t1 = new Transform3D();
|
static
|
//transient
|
Transform3D _t2 = new Transform3D();
|
static
|
//transient
|
Transform3D _transTF = new Transform3D();
|
static
|
//transient
|
Transform3D _tworld = new Transform3D();
|
static
|
//transient
|
Vector3d _trans = new Vector3d();
|
static
|
//transient
|
Vector3d _trans0 = new Vector3d();
|
|
/**
|
* Sets this bone/joint to the translation/rotation of the given frame.
|
* Called in the animation loop.
|
*
|
* @param frame
|
* Frame number
|
*/
|
public void setPose(Object3D bone, int frame, float[] data, boolean fade) //, Point3d offsetTrans)
|
{
|
if (!live) // aout 2013
|
return;
|
|
cVector temp = new cVector();
|
cVector temp0 = new cVector();
|
double pos[] = new double[3];
|
|
// Object3D obj = get(0);
|
// Object3D gp = this.parent;
|
// this.parent = null;
|
// obj.GlobalTransformInv();
|
// this.parent = gp;
|
//
|
// CameraPane.selectedpoint.toParent[3][0] = obj.globalTransform[3][0];
|
// CameraPane.selectedpoint.toParent[3][1] = obj.globalTransform[3][1];
|
// CameraPane.selectedpoint.toParent[3][2] = obj.globalTransform[3][2];
|
|
// System.out.println("setPose " + frame);
|
// _lastFrame = frame;
|
// do something if joint has at least one DOF
|
if (bone._numDOF > 0)
|
{
|
int offset = frame * bone._numDOF;
|
//if (offset + bone._numDOF - 1 < data.length)
|
{
|
boolean hasTranslation = false;
|
if (bone._isRoot)
|
{
|
_trans.set(0, 0, 0);
|
}
|
_t1.setIdentity();
|
|
float boost = 1; // .2f;
|
|
// go through DOF in the specified order
|
for (int i = 0; i < bone._numDOF; i++)
|
{
|
boolean isRotation = false;
|
switch (bone._dof[i])
|
{
|
case RX:
|
_t2.rotX(data[offset + i] * boost);
|
bone.rx = data[offset + i];
|
isRotation = true;
|
break;
|
case RY:
|
double angle = data[offset + i];
|
|
// if (bone._isRoot && !firstframe && frame > 0)
|
// {
|
// // Object3D rot = bone.get(0);
|
// //
|
// // _t2.setIdentity();
|
// //
|
// // double tx = rot.toParent[3][0];
|
// // double ty = rot.toParent[3][1];
|
// // double tz = rot.toParent[3][2];
|
//
|
// temp.x = 1;
|
// temp.y = 0;
|
// temp.z = 0;
|
// //LA.xformDir(temp, rot.toParent, temp);
|
//
|
// // double
|
// angle = Math.atan2(-temp.z, temp.x);
|
//
|
// // _t2.rotY(angle);
|
// //
|
// // cJ3D.ResetTransform(rot, _t2);
|
// //
|
// // rot.toParent[3][0] = tx;
|
// // rot.toParent[3][1] = ty;
|
// // rot.toParent[3][2] = tz;
|
// }
|
//continue;
|
_t2.rotY(angle * boost);
|
bone.ry = angle;
|
isRotation = true;
|
break;
|
case RZ:
|
_t2.rotZ(data[offset + i] * boost);
|
bone.rz = data[offset + i];
|
isRotation = true;
|
break;
|
case TX:
|
_trans.x += //bone._scale *
|
data[offset + i];
|
bone.tx = data[offset + i];
|
hasTranslation = true;
|
//isRotation = false;
|
break;
|
case TY:
|
_trans.y += //bone._scale *
|
data[offset + i];
|
bone.ty = data[offset + i];
|
hasTranslation = true;
|
//isRotation = false;
|
break;
|
case TZ:
|
_trans.z += //bone._scale *
|
data[offset + i];
|
bone.tz = data[offset + i];
|
hasTranslation = true;
|
//isRotation = false;
|
break;
|
}
|
|
// multiply rotations
|
if (isRotation)
|
{
|
// fixed axes: multiply from left
|
if (bone._rotationType == FIXED_AXES)
|
{
|
if (i > 0)
|
{
|
_t2.mul(_t1);
|
}
|
_t1.set(_t2); // = new Transform3D(_t2);
|
} else
|
{
|
// moving axes: multiply from right
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
}
|
}
|
}
|
|
if (bone._isRoot && firsttimeafterload && frame > 0)
|
{
|
assert(bone == get(0));
|
}
|
|
// set rotation
|
//if (bone._rotationEnabled)
|
if (//frame > 0 &&
|
!bone.skipmocap &&
|
!bone.name.contains("head") &&
|
// !bone.name.contains("rFoot") &&
|
// !bone.name.contains("lFoot") &&
|
!bone.name.contains("Pinky") &&
|
!bone.name.contains("Ring") &&
|
!bone.name.contains("Mid") &&
|
!bone.name.contains("Index") &&
|
!bone.name.contains("Thumb")
|
)
|
{
|
//_rotTG.setTransform(_t1);
|
if (!bone.name.contains("abdomen"))
|
{
|
cJ3D.ResetTransform(bone.get(0), _t1); // skip translation node
|
}
|
else
|
{
|
// if (get(0).link2master) // "hip" rotation
|
// _t1.setIdentity();
|
|
cJ3D.ResetTransform(bone.get(0), _t1); // skip translation node
|
}
|
}
|
|
// add offset to translation of root
|
// if (offsetTrans != null && _parent == null)
|
// {
|
// _trans.add(offsetTrans);
|
// hasTranslation = true;
|
// }
|
|
// set translation
|
// (currently only possible for root bone! see mocapdata.com data)
|
if (bone._isRoot && link2master) // && !GetFileRoot().link2master)
|
{
|
if (hasTranslation) // && _translationEnabled)
|
{
|
// CROWD
|
//cJ3D.GetTranslation(bone, _trans0);
|
//_trans.add(_trans0);
|
_transTF.setIdentity();
|
_transTF.setTranslation(_trans);
|
// System.err.println("Translate: " + _transTF);
|
//_transTG.setTransform(_transTF);
|
cJ3D.ResetTransform(bone, _transTF); // translation node
|
}
|
}
|
|
if (bone._isRoot && firsttimeafterload && frame > 0)
|
{
|
assert(bone == get(0));
|
|
// LA.matConcat(bone.toParent, bone.get(0).toParent, tempmatrix); // B' * C'
|
//
|
// // find A' : A'B'C' == ABC
|
// LA.matInvert(tempmatrix, tempmatrixinv);
|
// LA.matConcat(originmatrix, tempmatrixinv, toParent); // ABC * (B'*C')^-1
|
//
|
// LA.matConcat(bone.fromParent, bone.get(0).fromParent, tempmatrix); // B' * C'
|
//
|
// LA.matInvert(tempmatrix, tempmatrixinv);
|
// LA.matConcat(originmatrixinv, tempmatrixinv, fromParent); // ABC * (B'*C')^-1
|
//
|
|
Invariants();
|
bone.Invariants();
|
bone.get(0).Invariants();
|
// if (origin == null)
|
// origin = null;
|
origin.Invariants();
|
origin0.Invariants();
|
|
|
LA.matConcat(bone.get(0).fromParent, bone.fromParent, tempmatrix); // B' * C'
|
LA.matConcat(origin.toParent, tempmatrix, toParent);
|
|
// matrixToEulerXYZ(toParent, temp);
|
// matrixToEulerXYZ(bone.get(0).toParent, temp);
|
//
|
//// Quaternion quat = new Quaternion();
|
//// float[] angles = new float[3];
|
//// angles[0] = (float)-temp.x;
|
//// angles[1] = (float)-temp.y;
|
//// angles[2] = (float)-temp.z;
|
//// quat.fromAngles(angles);
|
//// quat.toAngles(angles);
|
// _t1.setIdentity();
|
// _t2.setIdentity();
|
// _t2.rotZ(-temp.z);
|
// _t1.mul(_t2);
|
// _t2.setIdentity();
|
// _t2.rotY(-temp.y);
|
// _t1.mul(_t2);
|
// _t2.setIdentity();
|
// _t2.rotX(-temp.x);
|
// _t1.mul(_t2);
|
// _t2.setIdentity();
|
//
|
// // double[][] rot = bone.get(0).
|
// // toParent;
|
//
|
// Quat4d quat = new Quat4d();
|
// Matrix4d mat4d = new Matrix4d();
|
//
|
// _t1.get(mat4d);
|
//
|
// mat4d.get(quat);
|
|
if (true) // hip correction
|
{
|
_t2.setIdentity();
|
|
double tx = toParent[3][0];
|
double ty = toParent[3][1];
|
double tz = toParent[3][2];
|
|
temp.x = 1;
|
temp.y = 0;
|
temp.z = 0;
|
LA.xformDir(temp, toParent, temp);
|
|
double angle = Math.atan2(-temp.z, temp.x);
|
|
// HIP ROTATION!!!
|
_t2.rotY(angle);
|
postponeangle = angle;
|
|
// _t1.setIdentity();
|
// _t1.rotX(temp.x);
|
// _t2.mul(_t1);
|
//
|
// _t1.setIdentity();
|
// _t1.rotY(temp.y);
|
// _t2.mul(_t1);
|
//
|
// _t1.setIdentity();
|
// _t1.rotZ(temp.z);
|
// _t2.mul(_t1);
|
|
cJ3D.ResetTransform(this, _t2);
|
|
toParent[3][0] = tx;
|
toParent[3][1] = ty;
|
toParent[3][2] = tz;
|
}
|
|
//????? LA.matConcat(bone.get(0).toParent, bone.toParent, tempmatrix); // B' * C'
|
// LA.matConcat(origin.fromParent, tempmatrix, fromParent);
|
|
LA.matInvert(toParent, fromParent);
|
|
Invariants();
|
bone.Invariants();
|
bone.get(0).Invariants();
|
origin.Invariants();
|
origin0.Invariants();
|
|
// LA.matInvert(fromParent, tempmatrix);
|
//
|
//// LA.matConcat(originmatrix0, bone.fromParent, tempmatrix);
|
//// toParent[3][0] = tempmatrix[3][0];
|
//// toParent[3][1] = tempmatrix[3][1];
|
//// toParent[3][2] = tempmatrix[3][2];
|
//// LA.matConcat(originmatrixinv0, bone.toParent, tempmatrix);
|
//// fromParent[3][0] = tempmatrix[3][0];
|
//// fromParent[3][1] = tempmatrix[3][1];
|
//// fromParent[3][2] = tempmatrix[3][2];
|
|
// LA.matConcat(originmatrix0, bone.fromParent, toParent);
|
// LA.matConcat(originmatrixinv0, bone.toParent, fromParent);
|
|
firsttimeafterload = false;
|
|
// temp.x = data[0];
|
// temp.y = data[1];
|
// temp.z = data[2];
|
LA.xformPos(temp, origin0.toParent, temp0);
|
|
// bone.GlobalTransformInv();
|
|
temp0.x = bone.globalTransform[3][0];
|
temp0.y = bone.globalTransform[3][1];
|
temp0.z = bone.globalTransform[3][2];
|
|
// MEGA TEST
|
fromParent[3][0] = 0;
|
fromParent[3][1] = 0;
|
fromParent[3][2] = 0;
|
LA.matInvert(fromParent, toParent);
|
|
LA.xformPos(temp0, fromParent, temp);
|
//LA.xformPos(temp, bone.fromParent, temp);
|
// float[] data;
|
|
pos[0] = temp.x;
|
pos[1] = temp.y;
|
pos[2] = temp.z;
|
|
// CameraPane.selectedpoint.toParent[3][0] = temp0.x;
|
// CameraPane.selectedpoint.toParent[3][1] = temp0.y;
|
// CameraPane.selectedpoint.toParent[3][2] = temp0.z;
|
|
int numframes = bvh.animation.getNumFrames();
|
|
int b = 0;
|
{
|
assert(data == bvh.animation.getBoneData(b));
|
|
int dof = 6; /// data.length/numframes;
|
|
// System.err.println("Bone #" + b + ": dof = " + dof);
|
int theframe = 0; // baseframe;
|
|
int frame3 = theframe*dof;
|
|
// for (int f=theframe; --f>=0;)
|
// {
|
// int f3 = f*dof;
|
//
|
// for (int i=dof/*3!*/; --i>=0;)
|
// {
|
// data[f3 + i] += pos[i] - data[frame3 + i];
|
// }
|
// }
|
for (int f=numframes; --f>=theframe;)
|
{
|
int f3 = f*dof;
|
|
if (f == theframe)
|
f = theframe;
|
|
// temp.x = data[f3];
|
// temp.y = data[f3 + 1];
|
// temp.z = data[f3 + 2];
|
// LA.xformPos(temp, originmatrix0, temp);
|
// LA.xformPos(temp, fromParent, temp);
|
// data[f3] = (float)temp.x;
|
// data[f3 + 1] = (float)temp.y;
|
// data[f3 + 2] = (float)temp.z;
|
|
for (int i=3/*dof*/; --i>=0;)
|
{
|
//if (i == 4)
|
// continue;
|
// if (i >= 4)
|
// {
|
// while (data[frame3 + i] < 0)
|
// {
|
// data[frame3 + i] += Math.PI*2;
|
// }
|
// while (data[f3 + i] < 0)
|
// {
|
// data[f3 + i] += Math.PI*2;
|
// }
|
// while (pos[i] < 0)
|
// {
|
// pos[i] += Math.PI*2;
|
// }
|
// }
|
|
data[f3 + i] += pos[i] - data[frame3 + i];
|
|
// if (f < 3)
|
// {
|
// System.out.print(data[f3 + i] + " ");
|
// }
|
}
|
// if (f < 3)
|
// {
|
// System.out.println();
|
// }
|
}
|
}
|
|
// temp.x = data[0];
|
// temp.y = data[1];
|
// temp.z = data[2];
|
//
|
// LA.xformPos(temp, toParent, temp);
|
//
|
// temp.x = data[0];
|
// temp.y = data[1];
|
// temp.z = data[2];
|
|
if (fade && currentbones != null && !CameraPane.fullreset)
|
{
|
Quat4d quatstart = new Quat4d();
|
Quat4d quatend = new Quat4d();
|
Quat4d quat = new Quat4d();
|
Matrix4d mat4d = new Matrix4d();
|
double[][] rot = new double[4][4];
|
|
for (b=numbones; --b>=1;) // HIP ROTATION: 0;)
|
{
|
int hipindex = get(0)._index;
|
|
// assert(get(0).get(0)._isHip);
|
|
// if (b == hipindex)
|
// continue;
|
|
data = bvh.animation.getBoneData(b);
|
|
int dof = 3; // data.length/numframes;
|
|
// assert(dof == 3);
|
|
int boneframe3 = b*3; // dof;
|
|
// System.err.println("Bone #" + b + ": dof = " + dof);
|
int fadein = 45; // 120;
|
|
// if (b == hipindex)
|
// fadein *= 5;
|
|
if (fadein > numframes)
|
fadein = 0; // numframes;
|
|
for (int fi=fadein; --fi>=0;)
|
//for (int f=numframes; --f>=0;)
|
{
|
int f = fi + GetFirstFrame();
|
|
int f3 = f*dof;
|
|
float k = fi;
|
|
k /= fadein - 1;
|
|
if (Float.isNaN(k))
|
k = 1; // 0/0
|
|
k = (float) (-Math.cos(k*Math.PI));
|
k += 1;
|
k /= 2;
|
|
int start = 3;
|
int end = 0;
|
|
if (dof == 6)
|
{
|
start = 6;
|
end = 3;
|
}
|
|
if (b == 0)
|
{
|
_t1.setIdentity();
|
_t2.setIdentity();
|
_t2.rotZ(currentbones[boneframe3 + start-1-end]);
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
if (b == 0)
|
{
|
_t2.rotY(currentbones[boneframe3 + start-2-end]);
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
_t2.rotX(currentbones[boneframe3 + start-3-end]);
|
}
|
else
|
{
|
_t2.rotX(currentbones[boneframe3 + start-2-end]);
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
_t2.rotY(currentbones[boneframe3 + start-3-end]);
|
}
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
|
_t1.get(mat4d);
|
|
mat4d.get(quatstart);
|
|
_t1.setIdentity();
|
_t2.setIdentity();
|
_t2.rotZ(data[f3 + start-1]);
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
if (b == 0)
|
{
|
_t2.rotY(data[f3 + start-2]);
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
_t2.rotX(data[f3 + start-3]);
|
}
|
else
|
{
|
_t2.rotX(data[f3 + start-2]);
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
_t2.rotY(data[f3 + start-3]);
|
}
|
|
_t1.mul(_t2);
|
_t2.setIdentity();
|
|
_t1.get(mat4d);
|
|
mat4d.get(quatend);
|
|
double dot = quatstart.x*quatend.x +
|
quatstart.y*quatend.y +
|
quatstart.z*quatend.z +
|
quatstart.w*quatend.w
|
;
|
|
if (dot < 0)
|
{
|
quatend.x *= -1;
|
quatend.y *= -1;
|
quatend.z *= -1;
|
quatend.w *= -1;
|
}
|
|
// k = 1-k;
|
|
quat.x = (1-k)*quatstart.x + k*quatend.x;
|
quat.y = (1-k)*quatstart.y + k*quatend.y;
|
quat.z = (1-k)*quatstart.z + k*quatend.z;
|
quat.w = (1-k)*quatstart.w + k*quatend.w;
|
|
quat.normalize();
|
|
mat4d.set(quat);
|
|
cJ3D.SetTransform(rot, mat4d);
|
|
matrixToEulerXYZ(rot, temp);
|
|
data[f3 + start-1] = -(float)temp.z;
|
|
if (b == 0)
|
{
|
data[f3 + start-2] = -(float)temp.y;
|
data[f3 + start-3] = -(float)temp.x;
|
}
|
else
|
{
|
data[f3 + start-2] = -(float)temp.x;
|
data[f3 + start-3] = -(float)temp.y;
|
}
|
}
|
else
|
{
|
for (int i=start; --i>=end;)
|
{
|
//data[f3 + i] += pos[i] - data[frame3 + i];
|
// data[f3 + i] = k*data[f3+i] + (1-k)*
|
// currentbones[boneframe3 + i-end];
|
data[f3 + i] = (float)CurveAngle(data[f3+i], currentbones[boneframe3 + i-end], 1-k);
|
}
|
}
|
|
}
|
}
|
|
}
|
}
|
|
if (false) // doesn't change anything bone._isRoot)
|
{
|
// Reset orientation
|
_t2.setIdentity();
|
|
double tx = bone.toParent[3][0];
|
double ty = bone.toParent[3][1];
|
double tz = bone.toParent[3][2];
|
|
temp.x = 1;
|
temp.y = 0;
|
temp.z = 0;
|
LA.xformDir(temp, bone.toParent, temp);
|
|
double angle = Math.atan2(-temp.z, temp.x);
|
|
_t2.rotY(angle);
|
// _t1.setIdentity();
|
// _t1.rotX(temp.x);
|
// _t2.mul(_t1);
|
//
|
// _t1.setIdentity();
|
// _t1.rotY(temp.y);
|
// _t2.mul(_t1);
|
//
|
// _t1.setIdentity();
|
// _t1.rotZ(temp.z);
|
// _t2.mul(_t1);
|
|
cJ3D.ResetTransform(bone, _t2);
|
|
bone.toParent[3][0] = tx;
|
bone.toParent[3][1] = ty;
|
bone.toParent[3][2] = tz;
|
}
|
}
|
|
if (currentbones == null)
|
currentbones = new double[numbones*3];
|
|
for (int b=numbones; --b>=1;) // 0;)
|
//int b = 0;
|
{
|
data = bvh.animation.getBoneData(b);
|
|
int numframes = bvh.animation.getNumFrames();
|
|
int dof = 6; // data.length/numframes;
|
|
if (b > 0)
|
dof = 3;
|
|
//assert(dof == 3);
|
|
// System.err.println("Bone #" + b + ": dof = " + dof);
|
int theframe = // 1; // baseframe;
|
frame;
|
|
int frame3 = theframe*dof;
|
int bone3 = b*3; // dof;
|
|
int start = 3;
|
int end = 0;
|
|
if (dof == 6)
|
{
|
start = 6;
|
end = 3;
|
}
|
for (int i=start; --i>=end;)
|
{
|
currentbones[bone3 + i-end] = data[frame3 + i];
|
}
|
}
|
}
|
}
|
|
static int offset = 1; // 5; // 10; // 5; // 100; // skip initial "T"
|
|
void ReadBVH()
|
{
|
try
|
{
|
mocap.reader.BVHReader rd = new mocap.reader.BVHReader(0.75);
|
//System.err.println("Reset MOCAP fullname = " + fullname);
|
|
if (random && !CameraPane.FAST)
|
{
|
// suppose a random crowd. We need longer mocap data.
|
System.err.println("MOCAP EXTENSION");
|
rd.mocapextension = rd.EXTENSION;
|
endframe = 0; // aout 2013
|
}
|
|
bvh = rd.readFile(new java.io.File(fullname));
|
|
smoothed = false; // aout 2013
|
}
|
catch(Exception e)
|
{
|
e.printStackTrace();
|
}
|
}
|
|
void Reset()
|
{
|
if (fullname != null)
|
ReadBVH();
|
|
Object3D hip = get(0);
|
|
// assert(obj.name.equals("hip"));
|
|
Object3D rot = hip.get(0);
|
|
// assert(obj.name == null); // .equals("hip#"));
|
|
if (hip.toParent == null)
|
{
|
hip.toParent = LA.newMatrix();
|
hip.fromParent = LA.newMatrix();
|
}
|
|
hip.toParent[3][0] = hip.toParent[3][1] = hip.toParent[3][2] = 0;
|
hip.fromParent[3][0] = hip.fromParent[3][1] = hip.fromParent[3][2] = 0;
|
|
if (toParent == null)
|
{
|
toParent = LA.newMatrix();
|
fromParent = LA.newMatrix();
|
}
|
|
LA.matIdentity(toParent);
|
LA.matIdentity(fromParent);
|
LA.matIdentity(hip.toParent);
|
LA.matIdentity(hip.fromParent);
|
LA.matIdentity(rot.toParent);
|
LA.matIdentity(rot.fromParent);
|
|
SetPositionDelta(true, true, true);
|
|
baseframe = frame = 0; // T pose. No offset.
|
|
// aout 2013 endframe = 0; // june 2013
|
|
setPose(frame);
|
}
|
|
static int mocapsupport = 0;
|
|
int GetFirstFrame()
|
{
|
//assert(offset + beginframe == 0);
|
return offset + beginframe;
|
}
|
|
void Step()
|
{
|
// if (CameraPane.ABORTED)
|
// return;
|
|
int step = 1;
|
|
if (speedup) // fev 2014
|
step *= 2; // 4;
|
if (rewind) // mars 2014
|
step *= 4;
|
|
if (CameraPane.FAST) // && !CameraPane.HOLD)
|
step *= CameraPane.STEP;
|
|
//System.err.println("Step Mocap frame # " + frame);
|
//System.err.println("Step Mocap baseframe # " + baseframe);
|
baseframe += step;
|
|
//frame = baseframe;
|
frame += step;
|
|
// if (frame != baseframe)
|
// {
|
// //assert(frame == baseframe);
|
// System.err.print("Step Mocap frame # " + frame);
|
// System.err.println("; baseframe # " + baseframe);
|
// }
|
|
int lastframetest = endframe;
|
|
if (lastframetest == 0)
|
lastframetest = bvh.animation.getNumFrames();
|
|
if (baseframe >= lastframetest) // july 2013 // - GetFirstFrame())
|
{
|
System.out.println("MOCAP reset: " + this.GetFileRoot() + "; baseframe = " + baseframe + "; beginframe = " + beginframe + "; lastframe = " + lastframetest + "; fullname = " + fullname);
|
//int delta = frame - baseframe;
|
if (CameraPane.CROWD)
|
{
|
baseframe = GetFirstFrame(); // 0 initial point
|
}
|
else
|
//frame = baseframe + delta;
|
// SetPositionDelta(false, true, false); // ?? false);
|
{
|
if (support == null)
|
ObjEditor.LoadBVHFile(fullname, this, false, beginframe, lastframetest);
|
else
|
{
|
try
|
{
|
bvh = ((Mocap)support.get(mocapsupport++ % support.Size())).bvh;
|
|
SetPositionDelta(false, true, true);
|
}
|
catch (Exception e)
|
{
|
e.printStackTrace();
|
ObjEditor.LoadBVHFile(fullname, this, false, beginframe, lastframetest);
|
}
|
}
|
}
|
}
|
|
//SetPositionDelta(false);
|
|
if (frame >= bvh.animation.getNumFrames())
|
//baseframe =
|
frame = GetFirstFrame(); // 0; // offset; // initial point
|
|
//System.err.println("frame = " + frame);
|
if (lastframe != 0)
|
{
|
// may 2014
|
if (baseframe > lastframe || baseframe < firstframe)
|
{
|
frame = baseframe = firstframe;
|
}
|
}
|
|
setPose(frame);
|
}
|
|
// filters out bad input data
|
void SmoothAnimData()
|
{
|
//if (true)
|
// return;
|
|
float[] data; // = bvh.animation.getBoneData(0);
|
|
//int dim = data.length;
|
|
int numframes = bvh.animation.getNumFrames();
|
|
//assert(dim == numframes*6);
|
|
for (int b=numbones; --b>=0;) // 0;)
|
{
|
data = bvh.animation.getBoneData(b);
|
|
int loops = 1;
|
|
if (b == 0)
|
// if (random) // aout 2013 supersmooth for crowds
|
loops = 8;
|
|
float[] tmp = new float[data.length];
|
|
Quaternion quat_1 = new Quaternion();
|
Quaternion quat = new Quaternion();
|
Quaternion quat1 = new Quaternion();
|
|
float[] angles = new float[3];
|
|
for (int loop=loops; --loop>=0;)
|
{
|
System.arraycopy(data, 0, tmp, 0, tmp.length);
|
// if (dim == -1)
|
// dim = data.length;
|
// else
|
// assert(dim == data.length);
|
|
int dof = 6; // data.length/numframes;
|
|
if (b > 0)
|
dof = 3;
|
|
// System.err.println("Bone #" + b + ": dof = " + dof);
|
|
float clamp = 0.1f;
|
|
for (int f=numframes-1; --f>=1;) // offset;) // 1;)
|
{
|
int f3_1 = (f-1)*dof;
|
int f3 = f*dof;
|
int f31 = (f+1)*dof;
|
|
// for (int i=dof; --i>=0;)
|
// angles[i] = data[f3_1 + i];
|
// quat_1.fromAngles(angles);
|
//
|
// for (int i=dof; --i>=0;)
|
// angles[i] = data[f3 + i];
|
// quat.fromAngles(angles);
|
//
|
// for (int i=dof; --i>=0;)
|
// angles[i] = data[f31 + i];
|
// quat1.fromAngles(angles);
|
//
|
// quat.x = 0.25f*quat_1.x + 0.5f*quat.x + 0.25f*quat1.x;
|
// quat.y = 0.25f*quat_1.y + 0.5f*quat.y + 0.25f*quat1.y;
|
// quat.z = 0.25f*quat_1.z + 0.5f*quat.z + 0.25f*quat1.z;
|
// quat.w = 0.25f*quat_1.w + 0.5f*quat.w + 0.25f*quat1.w;
|
//
|
// quat.normalize();
|
// quat.toAngles(angles);
|
|
int start = 3;
|
int end = 3;
|
|
if (b == 0)
|
{
|
end = 0;
|
}
|
|
for (int i=start; --i>=end;)
|
{
|
tmp[f3 + i] = 0.25f*data[f3_1 + i] + 0.5f*data[f3 + i] + 0.25f*data[f31 + i];
|
// if (dof == 3)
|
// tmp[f3 + i] = angles[i];
|
// else
|
// {
|
// if (i < 3)
|
// {
|
// tmp[f3 + i] = 0.25f*data[f3_1 + i] + 0.5f*data[f3 + i] + 0.25f*data[f31 + i];
|
// }
|
// else
|
// {
|
// tmp[f3 + i] = angles[i-3];
|
// }
|
// }
|
|
// if (Math.abs((tmp[f3 + i] - data[f3 + i]) / data[f3 + i]) > 0.1)
|
// {
|
// // value is too different. probably bad data
|
// tmp[f3 + i] = tmp[f31 + i];
|
// }
|
}
|
|
// angles
|
if (b == 0)
|
{
|
start = 6;
|
end = 3;
|
}
|
else
|
{
|
start = 3;
|
end = 0;
|
}
|
|
for (int i=start; --i>=end;)
|
{
|
double A = data[f3_1 + i];
|
double B = data[f3 + i];
|
double C = data[f31 + i];
|
|
double T = CurveAngle(A,C,0.5);
|
tmp[f3 + i] = (float)CurveAngle(T,B,0.5);
|
|
// double AB = CurveAngle(A,B,0.5);
|
// double BC = CurveAngle(B,C,0.5);
|
//
|
// double AC = CurveAngle(AB,BC,0.5);
|
//
|
// assert(tmp[f3 + i] == AC);
|
}
|
}
|
|
System.arraycopy(tmp, 0, data, 0, tmp.length);
|
}
|
}
|
}
|
|
// filters out bad input data
|
void FilterAnimData()
|
{
|
float[] data; // = bvh.animation.getBoneData(0);
|
|
//int dim = data.length;
|
|
int numframes = bvh.animation.getNumFrames();
|
|
//assert(dim == numframes*6);
|
|
for (int b=numbones; --b>=0;)
|
{
|
data = bvh.animation.getBoneData(b);
|
|
int loops = 1;
|
|
// if (b == 0)
|
// loops = 10;
|
|
for (int loop=loops; --loop>=0;)
|
{
|
int dof = 6; // data.length/numframes;
|
|
if (b > 0)
|
dof = 3;
|
|
// System.err.println("Bone #" + b + ": dof = " + dof);
|
|
float[] average = new float[dof];
|
float[] variance = new float[dof];
|
|
for (int f=numframes; --f>1;)
|
{
|
int f3_1 = (f-1)*dof;
|
int f3 = f*dof;
|
|
for (int i=dof; --i>=0;)
|
{
|
average[i] += (data[f3 + i] - data[f3_1 + i]) / (numframes-1);
|
}
|
}
|
|
for (int f=numframes; --f>1;)
|
{
|
int f3_1 = (f-1)*dof;
|
int f3 = f*dof;
|
|
for (int i=dof; --i>=0;)
|
{
|
double diff = (data[f3 + i] - data[f3_1 + i]) - average[i];
|
|
variance[i] += diff*diff / (numframes-1);
|
}
|
}
|
|
// for (int f=numframes-1; --f>1;)
|
for (int f=2; f<numframes-1; f++)
|
{
|
int f3_1 = (f-1)*dof;
|
int f3 = f*dof;
|
|
for (int i=dof; --i>=0;)
|
{
|
double diff = (data[f3 + i] - data[f3_1 + i]) - average[i];
|
|
if (diff*diff > variance[i]/100)
|
{
|
// we consider the data as invalid
|
int f31 = (f+1)*dof;
|
|
float prev = data[f3_1 + i];
|
float next = data[f31 + i];
|
|
data[f3 + i] = (next + prev) / 2;
|
}
|
}
|
}
|
}
|
}
|
}
|
|
transient boolean firsttimeafterload; // be ready to set "this" matrix
|
//double[][] originmatrix = LA.newMatrix(); // ABC
|
//double[][] originmatrixinv = LA.newMatrix();
|
//double[][] originmatrix0 = LA.newMatrix(); // ABC
|
//double[][] originmatrixinv0 = LA.newMatrix();
|
|
//static
|
Object3D origin; // = new Object3D();
|
//static
|
Object3D origin0; // = new Object3D();
|
|
//static
|
Object3D transformnode; // = new Object3D();
|
|
//static
|
double[][] tempmatrix; // = LA.newMatrix(); // B'C'
|
//static
|
double[][] tempmatrixinv; // = LA.newMatrix();
|
|
static double[][] matrix = LA.newMatrix(); // = LA.newMatrix();
|
static double[][] matrix2 = LA.newMatrix(); // = LA.newMatrix();
|
|
//static
|
transient
|
double[] currentbones;
|
|
void SetPositionDelta(boolean reset, boolean rewind, boolean load)
|
{
|
if (marked)
|
return;
|
|
//assert(dim == numframes*6);
|
|
if (load)
|
for (int b=numbones; --b>=0;)
|
//int b = 0;
|
{
|
float[] thedata = bvh.animation.getBoneData(b);
|
|
int numframes = bvh.animation.getNumFrames();
|
|
int dof = 6; // thedata.length/numframes;
|
|
if (b > 0)
|
dof = 3;
|
|
//for (int f=numframes; --f>=offset;)
|
int firstframe = GetFirstFrame();
|
for (int f=0; f<numframes-firstframe; f++)
|
{
|
int f3 = f*dof;
|
|
//for (int i=3/*dof*/; --i>=0;)
|
for (int i=dof; --i>=0;)
|
{
|
//thedata[f3 + i] += thedata[i] - thedata[offset*dof + i];
|
thedata[f3 + i] = thedata[f3 + firstframe*dof + i];
|
}
|
}
|
}
|
|
// assert(thedata[0] == 0);
|
// assert(thedata[1] == 0);
|
// assert(thedata[2] == 0);
|
|
// assert(thedata[6] == 0);
|
// assert(thedata[7] == 0);
|
// assert(thedata[8] == 0);
|
|
Object3D obj = get(0);
|
|
// assert(obj.name.equals("hip"));
|
|
Object3D rot = obj.get(0);
|
|
// if (toParent == null)
|
// {
|
// toParent = LA.newMatrix();
|
// fromParent = LA.newMatrix();
|
// }
|
// LA.matCopy(rot.toParent, fromParent);
|
// LA.matCopy(rot.fromParent, toParent);
|
// // assert(obj.name == null); // .equals("hip#"));
|
|
Invariants();
|
obj.Invariants();
|
rot.Invariants();
|
|
if (origin == null)
|
{
|
origin = new Object3D();
|
origin0 = new Object3D();
|
//transformnode = new Object3D();
|
tempmatrix = LA.newMatrix();
|
tempmatrixinv = LA.newMatrix();
|
}
|
|
origin.Invariants();
|
origin0.Invariants();
|
|
if (toParent == null) // mars 2014
|
{
|
toParent = LA.newMatrix();
|
fromParent = LA.newMatrix();
|
}
|
|
LA.matConcat(toParent, obj.toParent, origin0.toParent); // AB
|
Object3D gp = this.parent;
|
this.parent = null;
|
obj.GlobalTransformInv();
|
this.parent = gp;
|
// LA.matConcat(obj.fromParent, fromParent, origin0.toParent); // AB
|
LA.matConcat(origin0.toParent, rot.toParent, origin.toParent); // ABC
|
|
// CameraPane.selectedpoint.toParent[3][0] = obj.globalTransform[3][0];
|
// CameraPane.selectedpoint.toParent[3][1] = obj.globalTransform[3][1];
|
// CameraPane.selectedpoint.toParent[3][2] = obj.globalTransform[3][2];
|
//
|
// LA.matConcat(fromParent, obj.fromParent, origin0.fromParent);
|
// LA.matConcat(origin0.fromParent, rot.fromParent, origin.fromParent);
|
LA.matInvert(origin0.toParent, origin0.fromParent);
|
LA.matInvert(origin.toParent, origin.fromParent);
|
|
Invariants();
|
obj.Invariants();
|
rot.Invariants();
|
origin.Invariants();
|
origin0.Invariants();
|
|
firsttimeafterload = true;
|
|
if (rewind)
|
Rewind();
|
|
setPose(obj, frame, bvh.animation.getBoneData(obj._index), true);
|
|
if (true)
|
return;
|
|
double pos[] = new double[6];
|
|
if (!reset)
|
{
|
assert(obj.tx == obj.toParent[3][0]);
|
assert(obj.ty == obj.toParent[3][1]);
|
assert(obj.tz == obj.toParent[3][2]);
|
|
pos[0] = obj.tx; // obj.toParent[3][0];
|
pos[1] = obj.ty; // obj.toParent[3][1];
|
pos[2] = obj.tz; // obj.toParent[3][2];
|
pos[3] = obj.rz;
|
pos[4] = obj.ry;
|
pos[5] = obj.rx;
|
}
|
|
cVector temp = new cVector();
|
temp.x = obj.globalTransform[3][0];
|
temp.y = obj.globalTransform[3][1];
|
temp.z = obj.globalTransform[3][2];
|
// LA.xformPos(temp, origin0.toParent, temp);
|
// System.err.println("POS0 = " + temp.x + ", " + temp.y + ", " + temp.z + ";");
|
|
pos[0] = temp.x;
|
pos[1] = temp.y;
|
pos[2] = temp.z;
|
|
// System.err.println("POS1 = " + pos[0] + ", " + pos[1] + ", " + pos[2] + ";");
|
|
float[] data; // = bvh.animation.getBoneData(0);
|
|
//int dim = data.length;
|
|
int numframes = bvh.animation.getNumFrames();
|
|
//assert(dim == numframes*6);
|
|
for (int b=numbones; --b>=1;)
|
//int b = 0;
|
{
|
data = bvh.animation.getBoneData(b);
|
|
int dof = 3; // data.length/numframes;
|
|
assert(dof == 3);
|
|
// System.err.println("Bone #" + b + ": dof = " + dof);
|
int theframe = // 1; // baseframe;
|
frame;
|
|
int frame3 = theframe*dof;
|
int boneframe3 = b*dof;
|
|
for (int i=3/*dof*/; --i>=0;)
|
{
|
currentbones[boneframe3 + i] = data[frame3 + i];
|
}
|
|
if (true)
|
continue;
|
|
// for (int f=theframe; --f>=0;)
|
// {
|
// int f3 = f*dof;
|
//
|
// for (int i=dof/*3!*/; --i>=0;)
|
// {
|
// data[f3 + i] += pos[i] - data[frame3 + i];
|
// }
|
// }
|
for (int f=numframes; --f>=theframe;)
|
{
|
int f3 = f*dof;
|
|
if (f == theframe)
|
f = theframe;
|
|
for (int i=3/*dof*/; --i>=0;)
|
{
|
//if (i == 4)
|
// continue;
|
// if (i >= 4)
|
// {
|
// while (data[frame3 + i] < 0)
|
// {
|
// data[frame3 + i] += Math.PI*2;
|
// }
|
// while (data[f3 + i] < 0)
|
// {
|
// data[f3 + i] += Math.PI*2;
|
// }
|
// while (pos[i] < 0)
|
// {
|
// pos[i] += Math.PI*2;
|
// }
|
// }
|
|
data[f3 + i] += pos[i] - data[frame3 + i];
|
}
|
}
|
}
|
|
// Rewind();
|
setPose(obj, frame, bvh.animation.getBoneData(obj._index), true);
|
}
|
|
void Rewind()
|
{
|
baseframe = frame = GetFirstFrame();
|
}
|
|
String GetToolTip()
|
{
|
int seconds = (frame * 1000 / 120);
|
|
return Integer.toString(frame) + " frames : " + (seconds/1000) + "." + (seconds/100%10) + "" + (seconds/10%10) + "" + (seconds%10) + " seconds"; // / 100.0;
|
}
|
|
transient boolean restarted; // CROWD issue?
|
|
void drawSelf(CameraPane display, Object3D /*Composite*/ root, boolean selected, boolean blocked)
|
{
|
//System.err.println("drawSelf Mocap frame # " + frame);
|
//System.err.println("drawSelf Mocap baseframe # " + baseframe);
|
// frame = baseframe;
|
|
// if (hide)
|
// return;
|
|
if (count <= 0) // || display.IsFreezed())
|
{
|
return;
|
}
|
|
// if (firstframe)
|
// return;
|
|
if (!restarted && /*display.restartframe &&*/
|
display.LIVE && live && display.drawMode == display.SHADOW) // FUCK
|
{
|
//display.restartframe = false;
|
restarted = true;
|
Step();
|
}
|
else
|
{
|
if (display.LIVE && live && display.drawMode != display.SHADOW)
|
restarted = false;
|
}
|
|
super.drawSelf(display, root, selected, blocked);
|
}
|
|
//////////////
|
|
|
public int Size()
|
{
|
//if (count == 0)
|
// return 0;
|
|
//if (filecontent == null)
|
// return 0;
|
|
return count>0?1:0; // GetObject().Size();
|
}
|
|
public int size()
|
{
|
//if (filecontent == null)
|
// return 0;
|
|
return count>0?1:0; // GetObject().Size();
|
}
|
|
public int size0()
|
{
|
//if (filecontent == null)
|
// return 0;
|
|
return count>=0?1:0; // GetObject().Size();
|
}
|
|
public Object3D reserve(int i)
|
{
|
// assert(i==0);
|
if (i != 0)
|
System.exit(-1);
|
return GetSkeleton(); // .reserve(i);
|
}
|
|
public Object3D reserve0(int i)
|
{
|
// assert(i==0);
|
if (i != 0)
|
System.exit(-1);
|
return GetSkeleton(); // .reserve(i);
|
}
|
|
public void release(int i)
|
{
|
if (i != 0)
|
System.exit(-1);
|
// assert(i==0);
|
// GetObject().release(i);
|
}
|
|
public Object3D get(int i)
|
{
|
if (i != 0)
|
System.exit(-1);
|
// assert(i==0);
|
return GetSkeleton(); //.get(i);
|
}
|
|
public int indexOf(Object3D obj)
|
{
|
if (obj == GetSkeleton())
|
return 0;
|
|
return -1;
|
//return GetObject().indexOf(obj);
|
}
|
|
void createEditWindow(GroupEditor callee, boolean newWindow)
|
{
|
//editWindow = (new SphereEditor(this, deepCopy(), callee)).GetEditor();
|
if (newWindow)
|
{
|
objectUI = new MocapEditor(this, deepCopy(), callee);
|
} else
|
{
|
objectUI = new MocapEditor(this, callee);
|
}
|
editWindow = objectUI.GetEditor();
|
}
|
}
|
