From 89c1ad67bc65d24ceadfa9e95f8c5515283f1e97 Mon Sep 17 00:00:00 2001
From: Normand Briere <nbriere@noware.ca>
Date: Sun, 01 Jul 2018 15:45:28 -0400
Subject: [PATCH] Reverse matrix order.
---
Mocap.java | 1009 +++++++++++++++++++++++++++++++++++++---------------------
1 files changed, 644 insertions(+), 365 deletions(-)
diff --git a/Mocap.java b/Mocap.java
index 99ef9dc..93dd7ac 100644
--- a/Mocap.java
+++ b/Mocap.java
@@ -26,6 +26,456 @@
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];
+
+ // AT*AR = A'T*BT*A'R*BR
+ //
+ LA.matInvert(hip.get(0).toParent, matrix);
+
+ //LA.matIdentity(matrix);
+ //LA.matTranslate(matrix, -poship.x, -poship.y, -poship.z);
+
+ LA.matConcat(toParent, matrix, toParent);
+
+// 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.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 WriteTo(java.io.Writer writer) throws Exception
{
@@ -705,7 +1155,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 +1371,7 @@
//groundz += sourcenode.parent.toParent[3][2] - posz;
}
- boolean footcontact = CameraPane.FOOTCONTACT; // false;
+ boolean footcontact = false; // CameraPane.FOOTCONTACT; // false;
if (footcontact && ground != -123456)
{
@@ -1220,6 +1670,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);
@@ -1563,6 +2014,7 @@
toParent = LA.newMatrix();
fromParent = LA.newMatrix();
}
+
LA.matConcat(toParent, rotsourcenode.toParent, toParent);
LA.matInvert(toParent, fromParent);
@@ -1698,7 +2150,7 @@
}
if (!smoothed)
- for (int i=4; --i>=0;)
+ for (int i=0; --i>=0;)
SmoothAnimData(); // much reduces shakiness
smoothed = true;
@@ -1780,12 +2232,26 @@
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);
+ if (true)
+ return;
+
+ cVector centroid = new cVector();
+ cVector floor = new cVector();
+
+ Object3D sourcenode = GetFileRoot();
+
+ boolean success; // = sourcenode.getCentroid(centroid);
// if (!success)
// {
// //assert(success);
@@ -1793,16 +2259,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
@@ -2050,7 +2506,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;
@@ -2058,6 +2514,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;
@@ -2187,15 +2670,15 @@
// set rotation
//if (bone._rotationEnabled)
if (//frame > 0 &&
- !bone.skipmocap &&
- !bone.name.contains("head") &&
+ !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")
+// !bone.name.contains("Pinky") &&
+// !bone.name.contains("Ring") &&
+// !bone.name.contains("Mid") &&
+// !bone.name.contains("Index") &&
+// !bone.name.contains("Thumb")
)
{
//_rotTG.setTransform(_t1);
@@ -2228,15 +2711,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));
@@ -2252,17 +2737,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);
@@ -2296,7 +2781,7 @@
//
// mat4d.get(quat);
- if (true) // hip correction
+ if (false) // hip correction
{
_t2.setIdentity();
@@ -2337,13 +2822,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);
//
@@ -2364,33 +2849,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;
{
@@ -2398,12 +2889,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;
@@ -2455,17 +2946,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];
}
}
@@ -2479,186 +2979,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];
@@ -2667,9 +2998,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);
@@ -2682,56 +3013,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()
{
@@ -2767,7 +3063,7 @@
// assert(obj.name.equals("hip"));
- Object3D rot = hip.get(0);
+ //Object3D rot = hip.get(0);
// assert(obj.name == null); // .equals("hip#"));
@@ -2777,8 +3073,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)
{
@@ -2790,25 +3086,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;
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);
}
@@ -2840,7 +3139,7 @@
baseframe += step;
//frame = baseframe;
- frame += step;
+ //frame += step;
// if (frame != baseframe)
// {
@@ -2880,7 +3179,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)
{
@@ -2889,13 +3191,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)
@@ -3193,6 +3499,8 @@
//static
transient
double[] currentbones;
+ double[] currenthip;
+
transient boolean inbetween;
void SetPositionDelta(boolean reset, boolean rewind, boolean load, boolean fade)
@@ -3203,38 +3511,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);
@@ -3244,11 +3523,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)
// {
@@ -3260,7 +3539,7 @@
// // assert(obj.name == null); // .equals("hip#"));
Invariants();
- obj.Invariants();
+ hip.Invariants();
rot.Invariants();
if (origin == null)
@@ -3281,16 +3560,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)
@@ -3313,41 +3592,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 + ";");
@@ -3431,7 +3710,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