From 02e145cb923d601395acc7f15ae9e13f85ef2fbb Mon Sep 17 00:00:00 2001
From: Normand Briere <nbriere@noware.ca>
Date: Mon, 02 Jul 2018 21:38:58 -0400
Subject: [PATCH] Hip orientation.
---
Mocap.java | 1141 ++++++++++++++++++++++++++++++++++++++------------------
1 files changed, 768 insertions(+), 373 deletions(-)
diff --git a/Mocap.java b/Mocap.java
index 88fa618..172ab78 100644
--- a/Mocap.java
+++ b/Mocap.java
@@ -26,6 +26,544 @@
public class Mocap extends Object3D
{
static final long serialVersionUID = 7437391692559853707L;
+
+ void Fade()
+ {
+ if (currentbones == null || CameraPane.fullreset)
+ return;
+
+ cVector temp = new cVector();
+
+ int numframes = bvh.animation.getNumFrames();
+
+ int b;
+ float[] data;
+ 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>=0;) // 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;
+
+ if (b == 0)
+ {
+ dof = 6;
+ }
+ // assert(dof == 3);
+
+ int boneframe3 = (b-1)*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)
+ {
+ 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], currenthip[i], 1-k);
+ }
+
+ // TODO: translation
+
+// _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);
+ }
+ }
+ }
+ }
+ }
+
+ // Updates the toParent matrix to keep the same position and orientation
+ // before resetting the mocap data.
+ void SetGlobalTransform()
+ {
+ //SetCurrentBones(frame);
+
+ cVector temp = new cVector();
+ cVector pos = new cVector();
+ cVector poship = new cVector();
+
+ double angleY = 0;
+ double angleYhip = 0;
+
+ Object3D hip = get(0);
+
+ if (hip.get(0).toParent == null)
+ {
+ hip.get(0).toParent = LA.newMatrix();
+ hip.get(0).fromParent = LA.newMatrix();
+ }
+
+ LA.matConcat(toParent, hip.get(0).toParent, matrix);
+ poship.x = matrix[3][0];
+ poship.y = matrix[3][1];
+ poship.z = matrix[3][2];
+
+ temp.x = 1;
+ temp.y = 0;
+ temp.z = 0;
+
+ LA.xformDir(temp, matrix, temp);
+
+ angleYhip = Math.atan2(-temp.z, temp.x);
+
+ LA.matIdentity(toParent);
+ LA.matYRotate(toParent, angleYhip);
+ LA.matTranslate(toParent, poship.x, poship.y, poship.z);
+
+// LA.matConcat(toParent, hip.get(0).toParent, toParent);
+
+ CameraPane.debugpoint.toParent[3][0] = poship.x;
+ CameraPane.debugpoint.toParent[3][1] = poship.y;
+ CameraPane.debugpoint.toParent[3][2] = poship.z;
+
+ LA.matInvert(toParent, fromParent);
+
+ LA.matIdentity(hip.get(0).toParent);
+ LA.matIdentity(hip.get(0).fromParent);
+
+// if (true)
+// return;
+
+ // Updates hip.get(0).toParent
+ setPose(hip, GetFirstFrame(), bvh.animation.getBoneData(hip._index));
+
+ // A = toParent; B = hip.get(0).toParent
+ // A'*B = A
+ // A' = A * B-1
+
+ poship.x = hip.get(0).toParent[3][0];
+ poship.y = hip.get(0).toParent[3][1];
+ poship.z = hip.get(0).toParent[3][2];
+
+ temp.x = 1;
+ temp.y = 0;
+ temp.z = 0;
+
+ LA.xformDir(temp, hip.get(0).toParent, temp);
+
+ angleYhip = Math.atan2(-temp.z, temp.x);
+
+ LA.matIdentity(matrix);
+ LA.matYRotate(matrix, angleYhip);
+ LA.matTranslate(matrix, poship.x, poship.y, poship.z);
+
+ //LA.matInvert(hip.get(0).toParent, matrix);
+ LA.matInvert(matrix, matrix);
+
+ //LA.matIdentity(matrix);
+ //LA.matTranslate(matrix, -poship.x, -poship.y, -poship.z);
+
+ LA.matConcat(toParent, matrix, toParent);
+
+
+ LA.matConcat(toParent, hip.get(0).toParent, matrix);
+ pos.x = hip.get(0).toParent[3][0];
+ pos.y = hip.get(0).toParent[3][1];
+ pos.z = hip.get(0).toParent[3][2];
+ LA.xformPos(new cVector(), hip.get(0).toParent, pos);
+ LA.xformPos(pos, toParent, pos);
+
+// temp.x = 1;
+// temp.y = 0;
+// temp.z = 0;
+//
+// LA.xformDir(temp, toParent, temp);
+//
+// angleY = Math.atan2(-temp.z, temp.x);
+//
+// LA.matIdentity(toParent);
+// LA.matYRotate(toParent, angleY - angleYhip);
+// LA.matTranslate(toParent, pos.x - poship.x, pos.y - poship.y, pos.z - poship.z);
+
+ CameraPane.debugpoint2.toParent[3][0] = pos.x;
+ CameraPane.debugpoint2.toParent[3][1] = pos.y;
+ CameraPane.debugpoint2.toParent[3][2] = pos.z;
+
+ CameraPane.debugpoint3.toParent[3][0] = poship.x;
+ CameraPane.debugpoint3.toParent[3][1] = poship.y;
+ CameraPane.debugpoint3.toParent[3][2] = poship.z;
+
+ poship.x = toParent[3][0];
+ poship.y = toParent[3][1];
+ poship.z = toParent[3][2];
+
+ CameraPane.debugpoint4.toParent[3][0] = poship.x;
+ CameraPane.debugpoint4.toParent[3][1] = poship.y;
+ CameraPane.debugpoint4.toParent[3][2] = poship.z;
+
+ LA.matInvert(toParent, fromParent);
+ }
+
+ void LoadData()
+ {
+ float[] thedata = bvh.animation.getBoneData(0);
+
+// thedata[0] = 0;
+// thedata[1] = 0;
+// thedata[2] = 0;
+// thedata[3] = 0;
+// thedata[4] = 0;
+// thedata[5] = 0;
+
+ if (true)
+ return;
+
+ for (int b=numbones; --b>=0;)
+ //int b = 0;
+ {
+ thedata = bvh.animation.getBoneData(b);
+
+ int numframes = bvh.animation.getNumFrames();
+
+ int dof = 6; // thedata.length/numframes;
+
+ if (b > 0)
+ dof = 3;
+
+ if (dof != thedata.length/numframes)
+ {
+ System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (thedata.length/numframes));
+ dof = thedata.length/numframes;
+ }
+
+ //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];
+ }
+ }
+ }
+ }
+
+ void ResetZero()
+ {
+ for (int b=numbones; --b>=0;)
+ {
+ float[] thedata = bvh.animation.getBoneData(b);
+
+ int numframes = bvh.animation.getNumFrames();
+
+ int dof = 6;
+
+ if (b > 0)
+ dof = 3;
+
+ for (int i=dof; --i>=0;)
+ {
+ thedata[i] = 0;
+ }
+ }
+ }
+
+ void SetCurrentBones(int frame)
+ {
+ if (frame == 0)
+ {
+ //frame = 1;
+ }
+
+ if (currentbones == null)
+ {
+ currentbones = new double[(numbones-1)*3];
+ currenthip = new double[6];
+ }
+
+ for (int b=numbones; --b>=1;) // HIP ROTATION: 0;)
+ //int b = 0;
+ {
+ float[] 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-1)*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];
+ }
+ }
+
+ float[] data = bvh.animation.getBoneData(0);
+
+ int dof = 6;
+
+ int theframe = frame;
+
+ int frame3 = theframe*dof;
+
+ int start = 6;
+ int end = 0;
+
+ for (int i=start; --i>=end;)
+ {
+ currenthip[i] = data[frame3 + i];
+ }
+ }
+
+ void SetHipOrientation()
+ {
+ Object3D hip = get(0);
+
+ LA.matConcat(toParent, hip.get(0).toParent, matrix);
+
+ cVector centroid = new cVector();
+ centroid.x = matrix[3][0];
+ centroid.y = matrix[3][1];
+ centroid.z = matrix[3][2];
+ this.getCentroid(centroid, true);
+
+ cVector goal = GetGoal(centroid);
+ if (goal != null)
+ {
+ System.err.println("GOAL change (" + this + "): " + goal + " (was " + goalx + ", " + goalz + ")");
+
+ goalx = goal.x;
+ goalz = goal.z;
+ targetx = targetz = 0;
+ }
+
+ if (goalx == 0 && goalz == 0)
+ {
+ // No target
+ if (ScriptNode.speaker != null)
+ {
+ Object3D sourcenode = GetFileRoot();
+ Object3D speakernode = ScriptNode.speaker.GetFileRoot();
+
+ //System.err.println("speaker is " + speakernode + "; last was " + ScriptNode.lastspeaker);
+
+ Object3D speaker = ScriptNode.speaker;
+
+ if (speakernode == sourcenode)
+ {
+ if (ScriptNode.lastspeaker != null)
+ {
+ speakernode = ScriptNode.lastspeaker.GetFileRoot();
+ speaker = ScriptNode.lastspeaker;
+ }
+ else
+ return;
+ }
+
+ cVector dst = new cVector();
+
+ boolean success = speakernode.getCentroid(dst); // , true);
+
+ if (!success)
+ new Exception().printStackTrace();
+
+ //speakernode.parent.TransformToWorld(dst);
+ //sourcenode.parent.TransformToLocal(dst);
+ this.parent.TransformToLocal(dst);
+
+ goalx = dst.x;
+ goalz = dst.z;
+ }
+ else
+ return;
+ }
+
+ cVector temp = new cVector();
+
+ temp.x = 1;
+ temp.y = 0;
+ temp.z = 0;
+
+ LA.xformDir(temp, matrix, temp);
+
+ double angleYhip = Math.atan2(-temp.z, temp.x);
+
+ double angleYtarget = Math.atan2(goalx - centroid.x, goalz - centroid.z);
+
+ LA.matIdentity(matrix);
+ LA.matTranslate(matrix, -centroid.x, -centroid.y, -centroid.z);
+ double angle = CurveAngle(0, angleYtarget - angleYhip, 1); // 0.1f);
+ LA.matYRotate(matrix, angle);
+ LA.matTranslate(matrix, centroid.x, centroid.y, centroid.z);
+
+ LA.matConcat(matrix, toParent, toParent);
+
+ LA.matInvert(toParent, fromParent);
+
+ CheckForAction(centroid);
+ }
void WriteTo(java.io.Writer writer) throws Exception
{
@@ -102,8 +640,8 @@
double targetx;
double targetz;
- double goalx;
- double goalz;
+ double goalx = -20;
+ double goalz = -10;
boolean followpath;
@@ -359,7 +897,7 @@
// //GetFileRoot();
// sept 2014: //
- GraphreeD.RENDERME = 3; // patch for Merge objects
+ GrafreeD.RENDERME = 3; // patch for Merge objects
float[] thedata = bvh.animation.getBoneData(0);
os.x = (float) (path.get(i).toParent[3][0]); // - thedata[frame*6]);
@@ -460,6 +998,8 @@
testvect.set(from);
testvect.sub(fromto.from);
+ testvect.y = 0; // PATCH
+
int factor = 1;
//if (CameraPane.FAST)
@@ -501,6 +1041,8 @@
testvect.set(from);
testvect.sub(fromto.from);
+
+ testvect.y = 0; // PATCH
int factor = 1;
@@ -705,7 +1247,7 @@
// skip first node: translation
// Object3D b = input.get(0);
- setPose(input, f, data.getBoneData(input._index), false); //, null);
+ setPose(input, f, data.getBoneData(input._index)); //, false); //, null);
}
//mocap.figure.Bone[] children = b.getChildren();
@@ -921,7 +1463,7 @@
//groundz += sourcenode.parent.toParent[3][2] - posz;
}
- boolean footcontact = CameraPane.FOOTCONTACT; // false;
+ boolean footcontact = false; // CameraPane.FOOTCONTACT; // false;
if (footcontact && ground != -123456)
{
@@ -938,6 +1480,7 @@
groundid = floorid;
+ // green
CameraPane.debugpoint.toParent[3][0] = groundx;
CameraPane.debugpoint.toParent[3][1] = floor.y;
CameraPane.debugpoint.toParent[3][2] = groundz;
@@ -998,7 +1541,7 @@
lastsoundtime = CameraPane.framecount;
- GraphreeD.hassound = true;
+ GrafreeD.hassound = true;
}
// else
// System.out.println("skipped");
@@ -1016,6 +1559,7 @@
// floor.z += posz;
// System.out.println("Allo " + sourcenode);
+ // purple
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;
@@ -1052,7 +1596,7 @@
v0.z = groundz - floor.z;
double distance = Math.sqrt(v0.dot(v0));
- double maxdist = 0.004;
+ double maxdist = 0.04;
if (distance > maxdist)
{
@@ -1078,8 +1622,27 @@
sourcenode.toParent = LA.newMatrix();
sourcenode.fromParent = LA.newMatrix();
}
+
+ v0.x = v0.z = 0;
+
+ this.get(0).TransformToWorld(v0);
+
+ // cyan
+ CameraPane.debugpoint3.toParent[3][0] = v0.x;
+ CameraPane.debugpoint3.toParent[3][1] = ground;
+ CameraPane.debugpoint3.toParent[3][2] = v0.z;
+
LA.matConcat(sourcenode.toParent, transformnode.toParent, sourcenode.toParent);
LA.matInvert(sourcenode.toParent, sourcenode.fromParent);
+
+ v0.x = v0.z = 0;
+
+ this.get(0).TransformToWorld(v0);
+
+ // red
+ CameraPane.debugpoint4.toParent[3][0] = v0.x;
+ CameraPane.debugpoint4.toParent[3][1] = ground;
+ CameraPane.debugpoint4.toParent[3][2] = v0.z;
}
}
@@ -1199,6 +1762,7 @@
src.x = 0;
src.y = 0;
src.z = 0;
+ LA.xformPos(src, get(0).get(0).toParent, src);
LA.xformPos(src, get(0).toParent, src);
LA.xformPos(src, toParent, src);
// LA.xformPos(src, rotsourcenode.toParent, src);
@@ -1542,6 +2106,7 @@
toParent = LA.newMatrix();
fromParent = LA.newMatrix();
}
+
LA.matConcat(toParent, rotsourcenode.toParent, toParent);
LA.matInvert(toParent, fromParent);
@@ -1677,7 +2242,7 @@
}
if (!smoothed)
- for (int i=4; --i>=0;)
+ for (int i=0; --i>=0;)
SmoothAnimData(); // much reduces shakiness
smoothed = true;
@@ -1759,12 +2324,28 @@
if (!live) // aout 2013
return;
- cVector centroid = new cVector();
- cVector floor = new cVector();
-
- Object3D sourcenode = GetFileRoot();
+ //bvh.skeleton.setPose(f, bvh.animation.getBoneData(bvh.skeleton.getIndex()), null);
+ try
+ {
+ SetPose(GetSkeleton(), bvh.animation, f);
+ SetCurrentBones(frame);
+ }
+ catch (Exception e)
+ {
+ e.printStackTrace();
+ }
- boolean success; // = sourcenode.getCentroid(centroid);
+ SetHipOrientation();
+
+ if (true)
+ return;
+
+ cVector centroid = new cVector();
+ cVector floor = new cVector();
+
+ Object3D sourcenode = GetFileRoot();
+
+ boolean success; // = sourcenode.getCentroid(centroid);
// if (!success)
// {
// //assert(success);
@@ -1772,16 +2353,6 @@
// System.out.println("No centroid for " + sourcenode);
// }
- //bvh.skeleton.setPose(f, bvh.animation.getBoneData(bvh.skeleton.getIndex()), null);
- try
- {
- SetPose(GetSkeleton(), bvh.animation, f);
- }
- catch (Exception e)
- {
- e.printStackTrace();
- }
-
if (sourcenode.parent == null && sourcenode.fileparent == null)
{
// No GFD
@@ -1835,7 +2406,7 @@
{
SetRotation(sourcenode, new cVector(floor), new cVector(centroid));
}
-
+
if (setposition)
{
if (floorobject != null)
@@ -2029,7 +2600,7 @@
* @param frame
* Frame number
*/
- public void setPose(Object3D bone, int frame, float[] data, boolean fade) //, Point3d offsetTrans)
+ public void setPose(Object3D bone, int frame, float[] data) //, boolean fade) //, Point3d offsetTrans)
{
if (!live) // aout 2013
return;
@@ -2037,6 +2608,33 @@
cVector temp = new cVector();
cVector temp0 = new cVector();
double pos[] = new double[3];
+ double angleY = 0;
+ double sinY = 0;
+ double cosY = 0;
+
+ if (false) // bone._isRoot && firsttimeafterload && frame > 0)
+ {
+ bone.get(0).GlobalTransformInv();
+
+ temp.x = bone.get(0).globalTransform[3][0];
+ temp.y = bone.get(0).globalTransform[3][1];
+ temp.z = bone.get(0).globalTransform[3][2];
+
+ pos[0] = temp.x;
+ pos[1] = temp.y;
+ pos[2] = temp.z;
+
+ temp.x = 1;
+ temp.y = 0;
+ temp.z = 0;
+
+ LA.xformDir(temp, bone.get(0).globalTransform, temp);
+
+ sinY = temp.z;
+ cosY = temp.x;
+
+ angleY = Math.atan2(-sinY, cosY);
+ }
// Object3D obj = get(0);
// Object3D gp = this.parent;
@@ -2167,14 +2765,14 @@
//if (bone._rotationEnabled)
if (//frame > 0 &&
!bone.skipmocap &&
- !bone.name.contains("head") &&
+ !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")
+// !bone.name.contains("Pinky") &&
+// !bone.name.contains("Ring") &&
+// !bone.name.contains("Mid") &&
+// !bone.name.contains("Index") &&
+// !bone.name.contains("Thumb")
)
{
//_rotTG.setTransform(_t1);
@@ -2207,15 +2805,17 @@
// CROWD
//cJ3D.GetTranslation(bone, _trans0);
//_trans.add(_trans0);
- _transTF.setIdentity();
+ //_transTF.setIdentity();
+ _transTF.set(_t1);
_transTF.setTranslation(_trans);
// System.err.println("Translate: " + _transTF);
//_transTG.setTransform(_transTF);
- cJ3D.ResetTransform(bone, _transTF, true); // translation node
+ cJ3D.ResetTransform(bone.get(0), _transTF, true); // translation node
+ //cJ3D.ResetTransform(bone, _transTF, true); // translation node
}
}
- if (bone._isRoot && firsttimeafterload && frame > 0)
+ if (false) // bone._isRoot && firsttimeafterload && frame > 0)
{
assert(bone == get(0));
@@ -2231,17 +2831,17 @@
// 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();
+ 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);
+
+//?? 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);
@@ -2275,7 +2875,7 @@
//
// mat4d.get(quat);
- if (true) // hip correction
+ if (false) // hip correction
{
_t2.setIdentity();
@@ -2316,13 +2916,13 @@
//????? LA.matConcat(bone.get(0).toParent, bone.toParent, tempmatrix); // B' * C'
// LA.matConcat(origin.fromParent, tempmatrix, fromParent);
- LA.matInvert(toParent, fromParent);
+//?? LA.matInvert(toParent, fromParent);
- Invariants();
- bone.Invariants();
- bone.get(0).Invariants();
- origin.Invariants();
- origin0.Invariants();
+ Invariants();
+ bone.Invariants();
+ bone.get(0).Invariants();
+ origin.Invariants();
+ origin0.Invariants();
// LA.matInvert(fromParent, tempmatrix);
//
@@ -2343,33 +2943,39 @@
// temp.x = data[0];
// temp.y = data[1];
// temp.z = data[2];
- LA.xformPos(temp, origin0.toParent, temp0);
+ LA.xformPos(temp, origin0.toParent, temp0); // NU
-// bone.GlobalTransformInv();
-
- temp0.x = bone.globalTransform[3][0];
- temp0.y = bone.globalTransform[3][1];
- temp0.z = bone.globalTransform[3][2];
-
+// bone.get(0).GlobalTransformInv();
+//
+// temp0.x = bone.get(0).globalTransform[3][0];
+// temp0.y = bone.get(0).globalTransform[3][1];
+// temp0.z = bone.get(0).globalTransform[3][2];
+//
// MEGA TEST // june 2014: necessary for precision? otherwise comment works
-fromParent[3][0] = 0;
-fromParent[3][1] = 0;
-fromParent[3][2] = 0;
-LA.matInvert(fromParent, toParent);
+//fromParent[3][0] = 0;
+//fromParent[3][1] = 0;
+//fromParent[3][2] = 0;
+//LA.matInvert(fromParent, toParent);
- LA.xformPos(temp0, fromParent, temp);
+ LA.xformPos(temp0, fromParent, temp); // NU
//LA.xformPos(temp, bone.fromParent, temp);
// float[] data;
- pos[0] = temp.x;
- pos[1] = temp.y;
- pos[2] = temp.z;
+// pos[0] = temp.x;
+// pos[1] = temp.y;
+// pos[2] = temp.z;
+//
+// temp.x = 1;
+// temp.y = 0;
+// temp.z = 0;
+//
+// LA.xformDir(temp, bone.get(0).globalTransform, temp);
// 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 numframes = bvh.animation.getNumFrames();
int b = 0;
{
@@ -2377,12 +2983,12 @@
int dof = 6; /// data.length/numframes;
- // july 2014
- if (dof != data.length/numframes)
- {
- System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (data.length/numframes));
- dof = data.length/numframes;
- }
+ // july 2014
+ if (dof != data.length/numframes)
+ {
+ System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (data.length/numframes));
+ dof = data.length/numframes;
+ }
int theframe = 0; // baseframe;
@@ -2434,17 +3040,26 @@
// }
// }
- data[f3 + i] += pos[i] - data[frame3 + i];
+ data[f3 + i] += /*pos[i]*/ - data[frame3 + i];
// if (f < 3)
// {
// System.out.print(data[f3 + i] + " ");
// }
}
-// if (f < 3)
-// {
-// System.out.println();
-// }
+
+ double t1 = data[f3 + 0];
+ double t2 = data[f3 + 2];
+
+ data[f3 + 0] = (float)(cosY*t1 - sinY*t2);
+ data[f3 + 2] = (float)(sinY*t1 + cosY*t2);
+
+ for (int i=3/*dof*/; --i>=0;)
+ {
+ data[f3 + i] += pos[i];
+ }
+
+ data[f3 + 4] += angleY - data[frame3 + 4];
}
}
@@ -2458,186 +3073,17 @@
// 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 (fade && currentbones != null && !CameraPane.fullreset)
+// {
+// Fade();
+// }
}
-
+
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];
@@ -2646,9 +3092,9 @@
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);
@@ -2661,56 +3107,21 @@
// _t1.setIdentity();
// _t1.rotZ(temp.z);
// _t2.mul(_t1);
-
+
cJ3D.ResetTransform(bone, _t2, false);
-
+
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];
- }
- }
+ // SetCurrentBones(frame);
}
}
- static int offset = 1; // 5; // 10; // 5; // 100; // skip initial "T"
+ // skip initial "T"
+ static int offset = 1; // 5; // 10; // 5; // 100;
void ReadBVH()
{
@@ -2739,14 +3150,14 @@
void Reset()
{
- if (fullname != null)
- ReadBVH();
+// if (fullname != null)
+// ReadBVH();
Object3D hip = get(0);
// assert(obj.name.equals("hip"));
- Object3D rot = hip.get(0);
+ //Object3D rot = hip.get(0);
// assert(obj.name == null); // .equals("hip#"));
@@ -2756,8 +3167,8 @@
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;
+ //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)
{
@@ -2769,25 +3180,28 @@
LA.matIdentity(fromParent);
// LA.matIdentity(hip.toParent);
// LA.matIdentity(hip.fromParent);
- if (rot.toParent == null)
- {
- // june 2014
- rot.toParent = LA.newMatrix();
- rot.fromParent = LA.newMatrix();
- }
- LA.matIdentity(rot.toParent);
- LA.matIdentity(rot.fromParent);
+// if (rot.toParent == null)
+// {
+// // june 2014
+// rot.toParent = LA.newMatrix();
+// rot.fromParent = LA.newMatrix();
+// }
+// LA.matIdentity(rot.toParent);
+// LA.matIdentity(rot.fromParent);
// july 2014
- goalx = goalz = 0;
+// goalx = goalz = 0;
targetx = targetz = 0;
- SetPositionDelta(true, true, true, false);
+ //SetPositionDelta(true, true, true, false);
+// LoadData();
+// Rewind();
baseframe = frame = 0; // T pose. No offset.
// aout 2013 endframe = 0; // june 2013
+ //ResetZero();
setPose(frame);
}
@@ -2819,7 +3233,7 @@
baseframe += step;
//frame = baseframe;
- frame += step;
+ //frame += step;
// if (frame != baseframe)
// {
@@ -2833,6 +3247,7 @@
if (lastframetest == 0)
lastframetest = bvh.animation.getNumFrames();
+ // WARNING: RESET DESTROYS EVERYTHING
if (baseframe >= lastframetest) // july 2013 // - GetFirstFrame())
{
System.out.println("MOCAP reset: " + this.GetFileRoot() +
@@ -2858,7 +3273,10 @@
{
bvh = ((Mocap)support.get(mocapsupport++ % support.Size())).bvh;
- SetPositionDelta(false, true, true, true);
+ // SetPositionDelta(false, true, true, true);
+ LoadData();
+ Rewind();
+ Fade();
}
catch (Exception e)
{
@@ -2867,13 +3285,17 @@
}
}
}
+
+ frame = GetFirstFrame();
}
+ else
+ frame += step;
//SetPositionDelta(false);
- if (frame >= bvh.animation.getNumFrames())
- //baseframe =
- frame = GetFirstFrame(); // 0; // offset; // initial point
+// if (frame >= bvh.animation.getNumFrames())
+// //baseframe =
+// frame = GetFirstFrame(); // 0; // offset; // initial point
//System.err.println("frame = " + frame);
if (lastframe != 0)
@@ -3171,6 +3593,8 @@
//static
transient
double[] currentbones;
+ double[] currenthip;
+
transient boolean inbetween;
void SetPositionDelta(boolean reset, boolean rewind, boolean load, boolean fade)
@@ -3181,38 +3605,9 @@
//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;
-
- if (dof != thedata.length/numframes)
- {
- System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (thedata.length/numframes));
- dof = thedata.length/numframes;
- }
-
- //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];
- }
- }
- }
+ {
+ LoadData();
+ }
// assert(thedata[0] == 0);
// assert(thedata[1] == 0);
@@ -3222,11 +3617,11 @@
// assert(thedata[7] == 0);
// assert(thedata[8] == 0);
- Object3D obj = get(0);
+ Object3D hip = get(0);
// assert(obj.name.equals("hip"));
- Object3D rot = obj.get(0);
+ Object3D rot = hip.get(0);
// if (toParent == null)
// {
@@ -3238,7 +3633,7 @@
// // assert(obj.name == null); // .equals("hip#"));
Invariants();
- obj.Invariants();
+ hip.Invariants();
rot.Invariants();
if (origin == null)
@@ -3259,16 +3654,16 @@
fromParent = LA.newMatrix();
}
- if (obj.toParent == null) // june 2014
+ if (hip.toParent == null) // june 2014
{
- obj.toParent = LA.newMatrix();
- obj.fromParent = LA.newMatrix();
+ hip.toParent = LA.newMatrix();
+ hip.fromParent = LA.newMatrix();
}
- LA.matConcat(toParent, obj.toParent, origin0.toParent); // AB
+ LA.matConcat(toParent, hip.toParent, origin0.toParent); // AB
Object3D gp = this.parent;
this.parent = null;
- obj.GlobalTransformInv();
+ hip.GlobalTransformInv();
this.parent = gp;
if (rot.toParent == null)
@@ -3291,41 +3686,41 @@
LA.matInvert(origin.toParent, origin.fromParent);
Invariants();
- obj.Invariants();
+ hip.Invariants();
rot.Invariants();
origin.Invariants();
origin0.Invariants();
- firsttimeafterload = true;
+ firsttimeafterload = false; // true;
if (rewind)
Rewind();
- setPose(obj, frame, bvh.animation.getBoneData(obj._index), fade);
+ setPose(hip, frame, bvh.animation.getBoneData(hip._index)); //, fade);
- if (true)
- return;
+ 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]);
+ assert(hip.tx == hip.toParent[3][0]);
+ assert(hip.ty == hip.toParent[3][1]);
+ assert(hip.tz == hip.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;
+ pos[0] = hip.tx; // obj.toParent[3][0];
+ pos[1] = hip.ty; // obj.toParent[3][1];
+ pos[2] = hip.tz; // obj.toParent[3][2];
+ pos[3] = hip.rz;
+ pos[4] = hip.ry;
+ pos[5] = hip.rx;
}
cVector temp = new cVector();
- temp.x = obj.globalTransform[3][0];
- temp.y = obj.globalTransform[3][1];
- temp.z = obj.globalTransform[3][2];
+ temp.x = hip.globalTransform[3][0];
+ temp.y = hip.globalTransform[3][1];
+ temp.z = hip.globalTransform[3][2];
// LA.xformPos(temp, origin0.toParent, temp);
// System.err.println("POS0 = " + temp.x + ", " + temp.y + ", " + temp.z + ";");
@@ -3409,7 +3804,7 @@
}
// Rewind();
- setPose(obj, frame, bvh.animation.getBoneData(obj._index), true);
+ setPose(hip, frame, bvh.animation.getBoneData(hip._index)); // , true);
}
void Rewind()
--
Gitblit v1.6.2