~nbriere/GrafreeD.git - Noware Git Repository

..	..	@@ -26,6 +26,450 @@
26	26	public class Mocap extends Object3D
27	27	{
28	28	static final long serialVersionUID = 7437391692559853707L;
	29	+
	30	+ void Fade()
	31	+ {
	32	+ if (true) // currentbones == null \|\| CameraPane.fullreset)
	33	+ return;
	34	+
	35	+ cVector temp = new cVector();
	36	+
	37	+ int numframes = bvh.animation.getNumFrames();
	38	+
	39	+ int b;
	40	+ float[] data;
	41	+ Quat4d quatstart = new Quat4d();
	42	+ Quat4d quatend = new Quat4d();
	43	+ Quat4d quat = new Quat4d();
	44	+ Matrix4d mat4d = new Matrix4d();
	45	+ double[][] rot = new double[4][4];
	46	+ for (b=numbones; --b>=0;) // HIP ROTATION: 0;)
	47	+ {
	48	+ // int hipindex = get(0)._index;
	49	+
	50	+// assert(get(0).get(0)._isHip);
	51	+
	52	+ // if (b == hipindex)
	53	+ // continue;
	54	+
	55	+ data = bvh.animation.getBoneData(b);
	56	+
	57	+ int dof = 3; // data.length/numframes;
	58	+
	59	+ if (b == 0)
	60	+ {
	61	+ dof = 6;
	62	+ }
	63	+ // assert(dof == 3);
	64	+
	65	+ int boneframe3 = (b-1)*3; // dof;
	66	+
	67	+ // System.err.println("Bone #" + b + ": dof = " + dof);
	68	+ int fadein = 45; // 120;
	69	+
	70	+ // if (b == hipindex)
	71	+ // fadein *= 5;
	72	+
	73	+ if (fadein > numframes)
	74	+ fadein = 0; // numframes;
	75	+
	76	+ for (int fi=fadein; --fi>=0;)
	77	+ //for (int f=numframes; --f>=0;)
	78	+ {
	79	+ int f = fi + GetFirstFrame();
	80	+
	81	+ int f3 = f*dof;
	82	+
	83	+ float k = fi;
	84	+
	85	+ k /= fadein - 1;
	86	+
	87	+ if (Float.isNaN(k))
	88	+ k = 1; // 0/0
	89	+
	90	+ k = (float) (-Math.cos(k*Math.PI));
	91	+ k += 1;
	92	+ k /= 2;
	93	+
	94	+ int start = 3;
	95	+ int end = 0;
	96	+
	97	+ if (dof == 6)
	98	+ {
	99	+ start = 6;
	100	+ end = 3;
	101	+ }
	102	+
	103	+ if (b == 0)
	104	+ {
	105	+ for (int i=start; --i>=end;)
	106	+ {
	107	+ //data[f3 + i] += pos[i] - data[frame3 + i];
	108	+// data[f3 + i] = kdata[f3+i] + (1-k)
	109	+// currentbones[boneframe3 + i-end];
	110	+ data[f3 + i] = (float)CurveAngle(data[f3+i], currenthip[i], 1-k);
	111	+ }
	112	+
	113	+ // TODO: translation
	114	+
	115	+// _t1.setIdentity();
	116	+// _t2.setIdentity();
	117	+// _t2.rotZ(currentbones[boneframe3 + start-1-end]);
	118	+// _t1.mul(_t2);
	119	+// _t2.setIdentity();
	120	+// if (b == 0)
	121	+// {
	122	+// _t2.rotY(currentbones[boneframe3 + start-2-end]);
	123	+// _t1.mul(_t2);
	124	+// _t2.setIdentity();
	125	+// _t2.rotX(currentbones[boneframe3 + start-3-end]);
	126	+// }
	127	+// else
	128	+// {
	129	+// _t2.rotX(currentbones[boneframe3 + start-2-end]);
	130	+// _t1.mul(_t2);
	131	+// _t2.setIdentity();
	132	+// _t2.rotY(currentbones[boneframe3 + start-3-end]);
	133	+// }
	134	+// _t1.mul(_t2);
	135	+// _t2.setIdentity();
	136	+//
	137	+// _t1.get(mat4d);
	138	+//
	139	+// mat4d.get(quatstart);
	140	+//
	141	+// _t1.setIdentity();
	142	+// _t2.setIdentity();
	143	+// _t2.rotZ(data[f3 + start-1]);
	144	+// _t1.mul(_t2);
	145	+// _t2.setIdentity();
	146	+// if (b == 0)
	147	+// {
	148	+// _t2.rotY(data[f3 + start-2]);
	149	+// _t1.mul(_t2);
	150	+// _t2.setIdentity();
	151	+// _t2.rotX(data[f3 + start-3]);
	152	+// }
	153	+// else
	154	+// {
	155	+// _t2.rotX(data[f3 + start-2]);
	156	+// _t1.mul(_t2);
	157	+// _t2.setIdentity();
	158	+// _t2.rotY(data[f3 + start-3]);
	159	+// }
	160	+//
	161	+// _t1.mul(_t2);
	162	+// _t2.setIdentity();
	163	+//
	164	+// _t1.get(mat4d);
	165	+//
	166	+// mat4d.get(quatend);
	167	+//
	168	+// double dot = quatstart.x*quatend.x +
	169	+// quatstart.y*quatend.y +
	170	+// quatstart.z*quatend.z +
	171	+// quatstart.w*quatend.w
	172	+// ;
	173	+//
	174	+// if (dot < 0)
	175	+// {
	176	+// quatend.x *= -1;
	177	+// quatend.y *= -1;
	178	+// quatend.z *= -1;
	179	+// quatend.w *= -1;
	180	+// }
	181	+//
	182	+// // k = 1-k;
	183	+//
	184	+// quat.x = (1-k)quatstart.x + kquatend.x;
	185	+// quat.y = (1-k)quatstart.y + kquatend.y;
	186	+// quat.z = (1-k)quatstart.z + kquatend.z;
	187	+// quat.w = (1-k)quatstart.w + kquatend.w;
	188	+//
	189	+// quat.normalize();
	190	+//
	191	+// mat4d.set(quat);
	192	+//
	193	+// cJ3D.SetTransform(rot, mat4d);
	194	+//
	195	+// matrixToEulerXYZ(rot, temp);
	196	+//
	197	+// data[f3 + start-1] = -(float)temp.z;
	198	+//
	199	+// if (b == 0)
	200	+// {
	201	+// data[f3 + start-2] = -(float)temp.y;
	202	+// data[f3 + start-3] = -(float)temp.x;
	203	+// }
	204	+// else
	205	+// {
	206	+// data[f3 + start-2] = -(float)temp.x;
	207	+// data[f3 + start-3] = -(float)temp.y;
	208	+// }
	209	+ }
	210	+ else
	211	+ {
	212	+ for (int i=start; --i>=end;)
	213	+ {
	214	+ //data[f3 + i] += pos[i] - data[frame3 + i];
	215	+// data[f3 + i] = kdata[f3+i] + (1-k)
	216	+// currentbones[boneframe3 + i-end];
	217	+ data[f3 + i] = (float)CurveAngle(data[f3+i], currentbones[boneframe3 + i/-end/], 1-k);
	218	+ }
	219	+ }
	220	+ }
	221	+ }
	222	+ }
	223	+
	224	+ // Updates the toParent matrix to keep the same position and orientation
	225	+ // before resetting the mocap data.
	226	+ void SetGlobalTransform()
	227	+ {
	228	+ SetCurrentBones(frame);
	229	+
	230	+ cVector temp = new cVector();
	231	+ cVector pos = new cVector();
	232	+ cVector poship = new cVector();
	233	+
	234	+ double angleY = 0;
	235	+ double angleYhip = 0;
	236	+
	237	+ Object3D hip = get(0);
	238	+
	239	+ LA.matConcat(toParent, hip.get(0).toParent, matrix);
	240	+ poship.x = matrix[3][0];
	241	+ poship.y = matrix[3][1];
	242	+ poship.z = matrix[3][2];
	243	+
	244	+ temp.x = 1;
	245	+ temp.y = 0;
	246	+ temp.z = 0;
	247	+
	248	+ LA.xformDir(temp, matrix, temp);
	249	+
	250	+ angleYhip = Math.atan2(-temp.z, temp.x);
	251	+
	252	+ LA.matIdentity(toParent);
	253	+ LA.matYRotate(toParent, angleYhip);
	254	+ LA.matTranslate(toParent, poship.x, poship.y, poship.z);
	255	+
	256	+// LA.matConcat(toParent, hip.get(0).toParent, toParent);
	257	+
	258	+ CameraPane.debugpoint.toParent[3][0] = poship.x;
	259	+ CameraPane.debugpoint.toParent[3][1] = poship.y;
	260	+ CameraPane.debugpoint.toParent[3][2] = poship.z;
	261	+
	262	+ LA.matInvert(toParent, fromParent);
	263	+
	264	+ LA.matIdentity(hip.get(0).toParent);
	265	+ LA.matIdentity(hip.get(0).fromParent);
	266	+
	267	+// if (true)
	268	+// return;
	269	+
	270	+ // Updates hip.get(0).toParent
	271	+ setPose(hip, GetFirstFrame(), bvh.animation.getBoneData(hip._index));
	272	+
	273	+ // A = toParent; B = hip.get(0).toParent
	274	+ // A'*B = A
	275	+ // A' = A * B-1
	276	+
	277	+ poship.x = hip.get(0).toParent[3][0];
	278	+ poship.y = hip.get(0).toParent[3][1];
	279	+ poship.z = hip.get(0).toParent[3][2];
	280	+
	281	+ // ATAR = A'TBTA'RBR
	282	+ //
	283	+ LA.matInvert(hip.get(0).toParent, matrix);
	284	+
	285	+ //LA.matIdentity(matrix);
	286	+ //LA.matTranslate(matrix, -poship.x, -poship.y, -poship.z);
	287	+
	288	+ LA.matConcat(toParent, matrix, toParent);
	289	+
	290	+// poship.x = hip.get(0).toParent[3][0];
	291	+// poship.y = hip.get(0).toParent[3][1];
	292	+// poship.z = hip.get(0).toParent[3][2];
	293	+//
	294	+// temp.x = 1;
	295	+// temp.y = 0;
	296	+// temp.z = 0;
	297	+//
	298	+// LA.xformDir(temp, hip.get(0).toParent, temp);
	299	+//
	300	+// angleYhip = Math.atan2(-temp.z, temp.x);
	301	+
	302	+ LA.matConcat(toParent, hip.get(0).toParent, matrix);
	303	+ pos.x = hip.get(0).toParent[3][0];
	304	+ pos.y = hip.get(0).toParent[3][1];
	305	+ pos.z = hip.get(0).toParent[3][2];
	306	+ LA.xformPos(new cVector(), hip.get(0).toParent, pos);
	307	+ LA.xformPos(pos, toParent, pos);
	308	+
	309	+// temp.x = 1;
	310	+// temp.y = 0;
	311	+// temp.z = 0;
	312	+//
	313	+// LA.xformDir(temp, toParent, temp);
	314	+//
	315	+// angleY = Math.atan2(-temp.z, temp.x);
	316	+//
	317	+// LA.matIdentity(toParent);
	318	+// LA.matYRotate(toParent, angleY - angleYhip);
	319	+// LA.matTranslate(toParent, pos.x - poship.x, pos.y - poship.y, pos.z - poship.z);
	320	+
	321	+ CameraPane.debugpoint2.toParent[3][0] = pos.x;
	322	+ CameraPane.debugpoint2.toParent[3][1] = pos.y;
	323	+ CameraPane.debugpoint2.toParent[3][2] = pos.z;
	324	+
	325	+ CameraPane.debugpoint3.toParent[3][0] = poship.x;
	326	+ CameraPane.debugpoint3.toParent[3][1] = poship.y;
	327	+ CameraPane.debugpoint3.toParent[3][2] = poship.z;
	328	+
	329	+ poship.x = toParent[3][0];
	330	+ poship.y = toParent[3][1];
	331	+ poship.z = toParent[3][2];
	332	+
	333	+ CameraPane.debugpoint4.toParent[3][0] = poship.x;
	334	+ CameraPane.debugpoint4.toParent[3][1] = poship.y;
	335	+ CameraPane.debugpoint4.toParent[3][2] = poship.z;
	336	+
	337	+ LA.matInvert(toParent, fromParent);
	338	+ }
	339	+
	340	+ void LoadData()
	341	+ {
	342	+ float[] thedata = bvh.animation.getBoneData(0);
	343	+
	344	+// thedata[0] = 0;
	345	+// thedata[1] = 0;
	346	+// thedata[2] = 0;
	347	+// thedata[3] = 0;
	348	+// thedata[4] = 0;
	349	+// thedata[5] = 0;
	350	+
	351	+ if (true)
	352	+ return;
	353	+
	354	+ for (int b=numbones; --b>=0;)
	355	+ //int b = 0;
	356	+ {
	357	+ thedata = bvh.animation.getBoneData(b);
	358	+
	359	+ int numframes = bvh.animation.getNumFrames();
	360	+
	361	+ int dof = 6; // thedata.length/numframes;
	362	+
	363	+ if (b > 0)
	364	+ dof = 3;
	365	+
	366	+ if (dof != thedata.length/numframes)
	367	+ {
	368	+ System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (thedata.length/numframes));
	369	+ dof = thedata.length/numframes;
	370	+ }
	371	+
	372	+ //for (int f=numframes; --f>=offset;)
	373	+ int firstframe = GetFirstFrame();
	374	+ for (int f=0; f<numframes-firstframe; f++)
	375	+ {
	376	+ int f3 = f*dof;
	377	+
	378	+ //for (int i=3/dof/; --i>=0;)
	379	+ for (int i=dof; --i>=0;)
	380	+ {
	381	+ //thedata[f3 + i] += thedata[i] - thedata[offset*dof + i];
	382	+ thedata[f3 + i] = thedata[f3 + firstframe*dof + i];
	383	+ }
	384	+ }
	385	+ }
	386	+ }
	387	+
	388	+ void ResetZero()
	389	+ {
	390	+ for (int b=numbones; --b>=0;)
	391	+ {
	392	+ float[] thedata = bvh.animation.getBoneData(b);
	393	+
	394	+ int numframes = bvh.animation.getNumFrames();
	395	+
	396	+ int dof = 6;
	397	+
	398	+ if (b > 0)
	399	+ dof = 3;
	400	+
	401	+ for (int i=dof; --i>=0;)
	402	+ {
	403	+ thedata[i] = 0;
	404	+ }
	405	+ }
	406	+ }
	407	+
	408	+ void SetCurrentBones(int frame)
	409	+ {
	410	+ if (frame == 0)
	411	+ {
	412	+ //frame = 1;
	413	+ }
	414	+
	415	+ if (currentbones == null)
	416	+ {
	417	+ currentbones = new double[(numbones-1)*3];
	418	+ currenthip = new double[6];
	419	+ }
	420	+
	421	+ for (int b=numbones; --b>=1;) // HIP ROTATION: 0;)
	422	+ //int b = 0;
	423	+ {
	424	+ float[] data = bvh.animation.getBoneData(b);
	425	+
	426	+ // int numframes = bvh.animation.getNumFrames();
	427	+
	428	+ int dof = 6; // data.length/numframes;
	429	+
	430	+ if (b > 0)
	431	+ dof = 3;
	432	+
	433	+ //assert(dof == 3);
	434	+
	435	+ // System.err.println("Bone #" + b + ": dof = " + dof);
	436	+ int theframe = // 1; // baseframe;
	437	+ frame;
	438	+
	439	+ int frame3 = theframe*dof;
	440	+ int bone3 = (b-1)*dof;
	441	+
	442	+ int start = 3;
	443	+ int end = 0;
	444	+
	445	+ if (dof == 6)
	446	+ {
	447	+ start = 6;
	448	+ //end = 3;
	449	+ }
	450	+
	451	+ for (int i=start; --i>=end;)
	452	+ {
	453	+ currentbones[bone3 + i/-end/] = data[frame3 + i];
	454	+ }
	455	+ }
	456	+
	457	+ float[] data = bvh.animation.getBoneData(0);
	458	+
	459	+ int dof = 6;
	460	+
	461	+ int theframe = frame;
	462	+
	463	+ int frame3 = theframe*dof;
	464	+
	465	+ int start = 6;
	466	+ int end = 0;
	467	+
	468	+ for (int i=start; --i>=end;)
	469	+ {
	470	+ currenthip[i] = data[frame3 + i];
	471	+ }
	472	+ }
29	473
30	474	void WriteTo(java.io.Writer writer) throws Exception
31	475	{
..	..	@@ -359,7 +803,7 @@
359	803	// //GetFileRoot();
360	804
361	805	// sept 2014: //
362		- GraphreeD.RENDERME = 3; // patch for Merge objects
	806	+ GrafreeD.RENDERME = 3; // patch for Merge objects
363	807	float[] thedata = bvh.animation.getBoneData(0);
364	808
365	809	os.x = (float) (path.get(i).toParent[3][0]); // - thedata[frame*6]);
..	..	@@ -705,7 +1149,7 @@
705	1149	// skip first node: translation
706	1150	// Object3D b = input.get(0);
707	1151
708		- setPose(input, f, data.getBoneData(input._index), false); //, null);
	1152	+ setPose(input, f, data.getBoneData(input._index)); //, false); //, null);
709	1153	}
710	1154
711	1155	//mocap.figure.Bone[] children = b.getChildren();
..	..	@@ -754,7 +1198,7 @@
754	1198	//long
755	1199	// june 2014 floorid = sourcenode.getFloor(floor); // , sourcenode.parent);
756	1200
757		- CameraPane.debugpoints[0].name = "Floor = " + floorid;
	1201	+ CameraPane.debugpoints[0].name = ""; // "Floor = " + 0; // floorid;
758	1202	CameraPane.debugpoints[0].toParent[3][0] = floor.x;
759	1203	CameraPane.debugpoints[0].toParent[3][1] = floor.y;
760	1204	CameraPane.debugpoints[0].toParent[3][2] = floor.z;
..	..	@@ -770,7 +1214,7 @@
770	1214
771	1215	this.count--; // currently drawing itself
772	1216
773		- CameraPane.debugpoints[7].name = "Centroid = " + groundid; // centroid;
	1217	+ CameraPane.debugpoints[7].name = ""; // Centroid = " + 0; // groundid; // centroid;
774	1218	CameraPane.debugpoints[7].toParent[3][0] = centroid.x;
775	1219	CameraPane.debugpoints[7].toParent[3][1] = centroid.y;
776	1220	CameraPane.debugpoints[7].toParent[3][2] = centroid.z;
..	..	@@ -795,7 +1239,7 @@
795	1239
796	1240	// jan 2014 sourcenode.parent.TransformToWorld(tmp);
797	1241
798		- CameraPane.debugpoints[1].name = "C+";
	1242	+ CameraPane.debugpoints[1].name = ""; // "C+";
799	1243	CameraPane.debugpoints[1].toParent[3][0] = tmp.x;
800	1244	CameraPane.debugpoints[1].toParent[3][1] = tmp.y;
801	1245	CameraPane.debugpoints[1].toParent[3][2] = tmp.z;
..	..	@@ -814,7 +1258,7 @@
814	1258	tmp.z = pinz;
815	1259	}
816	1260
817		- CameraPane.debugpoints[2].name = "Pin";
	1261	+ CameraPane.debugpoints[2].name = ""; // "Pin";
818	1262	CameraPane.debugpoints[2].toParent[3][0] = tmp.x;
819	1263	CameraPane.debugpoints[2].toParent[3][1] = tmp.y;
820	1264	CameraPane.debugpoints[2].toParent[3][2] = tmp.z;
..	..	@@ -822,7 +1266,7 @@
822	1266	//if (sourcenode.parent.parent != null)
823	1267	// jan 2014 sourcenode.parent.TransformToLocal(tmp); //, tmp);
824	1268
825		- CameraPane.debugpoints[3].name = "C-";
	1269	+ CameraPane.debugpoints[3].name = ""; // "C-";
826	1270	CameraPane.debugpoints[3].toParent[3][0] = tmp.x;
827	1271	CameraPane.debugpoints[3].toParent[3][1] = tmp.y;
828	1272	CameraPane.debugpoints[3].toParent[3][2] = tmp.z;
..	..	@@ -895,14 +1339,14 @@
895	1339	// if (sourcenode.parent.parent != null)
896	1340	// sourcenode.parent.parent.TransformToWorld(floor); //, floor);
897	1341
898		- CameraPane.debugpoints[5].name = "F++";
	1342	+ CameraPane.debugpoints[5].name = ""; // "F++";
899	1343	CameraPane.debugpoints[5].toParent[3][0] = floor.x;
900	1344	CameraPane.debugpoints[5].toParent[3][1] = floor.y;
901	1345	CameraPane.debugpoints[5].toParent[3][2] = floor.z;
902	1346
903	1347	floor.y = tempg;
904	1348
905		- CameraPane.debugpoints[6].name = "F+-";
	1349	+ CameraPane.debugpoints[6].name = ""; // "F+-";
906	1350	CameraPane.debugpoints[6].toParent[3][0] = floor.x;
907	1351	CameraPane.debugpoints[6].toParent[3][1] = floor.y;
908	1352	CameraPane.debugpoints[6].toParent[3][2] = floor.z;
..	..	@@ -921,7 +1365,7 @@
921	1365	//groundz += sourcenode.parent.toParent[3][2] - posz;
922	1366	}
923	1367
924		- boolean footcontact = CameraPane.FOOTCONTACT; // false;
	1368	+ boolean footcontact = false; // CameraPane.FOOTCONTACT; // false;
925	1369
926	1370	if (footcontact && ground != -123456)
927	1371	{
..	..	@@ -938,6 +1382,7 @@
938	1382
939	1383	groundid = floorid;
940	1384
	1385	+ // green
941	1386	CameraPane.debugpoint.toParent[3][0] = groundx;
942	1387	CameraPane.debugpoint.toParent[3][1] = floor.y;
943	1388	CameraPane.debugpoint.toParent[3][2] = groundz;
..	..	@@ -998,7 +1443,7 @@
998	1443
999	1444	lastsoundtime = CameraPane.framecount;
1000	1445
1001		- GraphreeD.hassound = true;
	1446	+ GrafreeD.hassound = true;
1002	1447	}
1003	1448	// else
1004	1449	// System.out.println("skipped");
..	..	@@ -1016,6 +1461,7 @@
1016	1461	// floor.z += posz;
1017	1462
1018	1463	// System.out.println("Allo " + sourcenode);
	1464	+ // purple
1019	1465	CameraPane.debugpoint2.toParent[3][0] = floor.x;// + posx;
1020	1466	CameraPane.debugpoint2.toParent[3][1] = ground; // floor.y;// + sourcenode.parent.toParent[3][1];
1021	1467	CameraPane.debugpoint2.toParent[3][2] = floor.z;// + posz;
..	..	@@ -1052,7 +1498,7 @@
1052	1498	v0.z = groundz - floor.z;
1053	1499
1054	1500	double distance = Math.sqrt(v0.dot(v0));
1055		- double maxdist = 0.004;
	1501	+ double maxdist = 0.04;
1056	1502
1057	1503	if (distance > maxdist)
1058	1504	{
..	..	@@ -1078,8 +1524,27 @@
1078	1524	sourcenode.toParent = LA.newMatrix();
1079	1525	sourcenode.fromParent = LA.newMatrix();
1080	1526	}
	1527	+
	1528	+ v0.x = v0.z = 0;
	1529	+
	1530	+ this.get(0).TransformToWorld(v0);
	1531	+
	1532	+ // cyan
	1533	+ CameraPane.debugpoint3.toParent[3][0] = v0.x;
	1534	+ CameraPane.debugpoint3.toParent[3][1] = ground;
	1535	+ CameraPane.debugpoint3.toParent[3][2] = v0.z;
	1536	+
1081	1537	LA.matConcat(sourcenode.toParent, transformnode.toParent, sourcenode.toParent);
1082	1538	LA.matInvert(sourcenode.toParent, sourcenode.fromParent);
	1539	+
	1540	+ v0.x = v0.z = 0;
	1541	+
	1542	+ this.get(0).TransformToWorld(v0);
	1543	+
	1544	+ // red
	1545	+ CameraPane.debugpoint4.toParent[3][0] = v0.x;
	1546	+ CameraPane.debugpoint4.toParent[3][1] = ground;
	1547	+ CameraPane.debugpoint4.toParent[3][2] = v0.z;
1083	1548	}
1084	1549	}
1085	1550
..	..	@@ -1199,6 +1664,7 @@
1199	1664	src.x = 0;
1200	1665	src.y = 0;
1201	1666	src.z = 0;
	1667	+ LA.xformPos(src, get(0).get(0).toParent, src);
1202	1668	LA.xformPos(src, get(0).toParent, src);
1203	1669	LA.xformPos(src, toParent, src);
1204	1670	// LA.xformPos(src, rotsourcenode.toParent, src);
..	..	@@ -1318,7 +1784,7 @@
1318	1784
1319	1785	rotsourcenode.TransformToWorld(test);
1320	1786
1321		- CameraPane.debugpoints[4].name = "ROT = "; // + test;
	1787	+ CameraPane.debugpoints[4].name = ""; // "ROT = "; // + test;
1322	1788	CameraPane.debugpoints[4].toParent[3][0] = test.x;
1323	1789	CameraPane.debugpoints[4].toParent[3][1] = test.y;
1324	1790	CameraPane.debugpoints[4].toParent[3][2] = test.z;
..	..	@@ -1542,6 +2008,7 @@
1542	2008	toParent = LA.newMatrix();
1543	2009	fromParent = LA.newMatrix();
1544	2010	}
	2011	+
1545	2012	LA.matConcat(toParent, rotsourcenode.toParent, toParent);
1546	2013	LA.matInvert(toParent, fromParent);
1547	2014
..	..	@@ -1677,7 +2144,7 @@
1677	2144	}
1678	2145
1679	2146	if (!smoothed)
1680		- for (int i=4; --i>=0;)
	2147	+ for (int i=0; --i>=0;)
1681	2148	SmoothAnimData(); // much reduces shakiness
1682	2149
1683	2150	smoothed = true;
..	..	@@ -1759,12 +2226,26 @@
1759	2226	if (!live) // aout 2013
1760	2227	return;
1761	2228
1762		- cVector centroid = new cVector();
1763		- cVector floor = new cVector();
1764		-
1765		- Object3D sourcenode = GetFileRoot();
	2229	+ //bvh.skeleton.setPose(f, bvh.animation.getBoneData(bvh.skeleton.getIndex()), null);
	2230	+ try
	2231	+ {
	2232	+ SetPose(GetSkeleton(), bvh.animation, f);
	2233	+ SetCurrentBones(frame);
	2234	+ }
	2235	+ catch (Exception e)
	2236	+ {
	2237	+ e.printStackTrace();
	2238	+ }
1766	2239
1767		- boolean success; // = sourcenode.getCentroid(centroid);
	2240	+ if (true)
	2241	+ return;
	2242	+
	2243	+ cVector centroid = new cVector();
	2244	+ cVector floor = new cVector();
	2245	+
	2246	+ Object3D sourcenode = GetFileRoot();
	2247	+
	2248	+ boolean success; // = sourcenode.getCentroid(centroid);
1768	2249	// if (!success)
1769	2250	// {
1770	2251	// //assert(success);
..	..	@@ -1772,16 +2253,6 @@
1772	2253	// System.out.println("No centroid for " + sourcenode);
1773	2254	// }
1774	2255
1775		- //bvh.skeleton.setPose(f, bvh.animation.getBoneData(bvh.skeleton.getIndex()), null);
1776		- try
1777		- {
1778		- SetPose(GetSkeleton(), bvh.animation, f);
1779		- }
1780		- catch (Exception e)
1781		- {
1782		- e.printStackTrace();
1783		- }
1784		-
1785	2256	if (sourcenode.parent == null && sourcenode.fileparent == null)
1786	2257	{
1787	2258	// No GFD
..	..	@@ -1835,7 +2306,7 @@
1835	2306	{
1836	2307	SetRotation(sourcenode, new cVector(floor), new cVector(centroid));
1837	2308	}
1838		-
	2309	+
1839	2310	if (setposition)
1840	2311	{
1841	2312	if (floorobject != null)
..	..	@@ -2029,7 +2500,7 @@
2029	2500	* @param frame
2030	2501	* Frame number
2031	2502	*/
2032		- public void setPose(Object3D bone, int frame, float[] data, boolean fade) //, Point3d offsetTrans)
	2503	+ public void setPose(Object3D bone, int frame, float[] data) //, boolean fade) //, Point3d offsetTrans)
2033	2504	{
2034	2505	if (!live) // aout 2013
2035	2506	return;
..	..	@@ -2037,6 +2508,33 @@
2037	2508	cVector temp = new cVector();
2038	2509	cVector temp0 = new cVector();
2039	2510	double pos[] = new double[3];
	2511	+ double angleY = 0;
	2512	+ double sinY = 0;
	2513	+ double cosY = 0;
	2514	+
	2515	+ if (false) // bone._isRoot && firsttimeafterload && frame > 0)
	2516	+ {
	2517	+ bone.get(0).GlobalTransformInv();
	2518	+
	2519	+ temp.x = bone.get(0).globalTransform[3][0];
	2520	+ temp.y = bone.get(0).globalTransform[3][1];
	2521	+ temp.z = bone.get(0).globalTransform[3][2];
	2522	+
	2523	+ pos[0] = temp.x;
	2524	+ pos[1] = temp.y;
	2525	+ pos[2] = temp.z;
	2526	+
	2527	+ temp.x = 1;
	2528	+ temp.y = 0;
	2529	+ temp.z = 0;
	2530	+
	2531	+ LA.xformDir(temp, bone.get(0).globalTransform, temp);
	2532	+
	2533	+ sinY = temp.z;
	2534	+ cosY = temp.x;
	2535	+
	2536	+ angleY = Math.atan2(-sinY, cosY);
	2537	+ }
2040	2538
2041	2539	// Object3D obj = get(0);
2042	2540	// Object3D gp = this.parent;
..	..	@@ -2166,15 +2664,15 @@
2166	2664	// set rotation
2167	2665	//if (bone._rotationEnabled)
2168	2666	if (//frame > 0 &&
2169		- !bone.skipmocap &&
2170		- !bone.name.contains("head") &&
	2667	+ !bone.skipmocap //&&
	2668	+ //!bone.name.contains("head") &&
2171	2669	// !bone.name.contains("rFoot") &&
2172	2670	// !bone.name.contains("lFoot") &&
2173		- !bone.name.contains("Pinky") &&
2174		- !bone.name.contains("Ring") &&
2175		- !bone.name.contains("Mid") &&
2176		- !bone.name.contains("Index") &&
2177		- !bone.name.contains("Thumb")
	2671	+// !bone.name.contains("Pinky") &&
	2672	+// !bone.name.contains("Ring") &&
	2673	+// !bone.name.contains("Mid") &&
	2674	+// !bone.name.contains("Index") &&
	2675	+// !bone.name.contains("Thumb")
2178	2676	)
2179	2677	{
2180	2678	//_rotTG.setTransform(_t1);
..	..	@@ -2207,15 +2705,17 @@
2207	2705	// CROWD
2208	2706	//cJ3D.GetTranslation(bone, _trans0);
2209	2707	//_trans.add(_trans0);
2210		- _transTF.setIdentity();
	2708	+ //_transTF.setIdentity();
	2709	+ _transTF.set(_t1);
2211	2710	_transTF.setTranslation(_trans);
2212	2711	// System.err.println("Translate: " + _transTF);
2213	2712	//_transTG.setTransform(_transTF);
2214		- cJ3D.ResetTransform(bone, _transTF, true); // translation node
	2713	+ cJ3D.ResetTransform(bone.get(0), _transTF, true); // translation node
	2714	+ //cJ3D.ResetTransform(bone, _transTF, true); // translation node
2215	2715	}
2216	2716	}
2217	2717
2218		- if (bone._isRoot && firsttimeafterload && frame > 0)
	2718	+ if (false) // bone._isRoot && firsttimeafterload && frame > 0)
2219	2719	{
2220	2720	assert(bone == get(0));
2221	2721
..	..	@@ -2231,17 +2731,17 @@
2231	2731	// LA.matConcat(originmatrixinv, tempmatrixinv, fromParent); // ABC * (B'*C')^-1
2232	2732	//
2233	2733
2234		- Invariants();
2235		- bone.Invariants();
2236		- bone.get(0).Invariants();
2237		- // if (origin == null)
2238		- // origin = null;
2239		- origin.Invariants();
2240		- origin0.Invariants();
	2734	+ Invariants();
	2735	+ bone.Invariants();
	2736	+ bone.get(0).Invariants();
	2737	+ // if (origin == null)
	2738	+ // origin = null;
	2739	+ origin.Invariants();
	2740	+ origin0.Invariants();
2241	2741
2242		-
2243		- LA.matConcat(bone.get(0).fromParent, bone.fromParent, tempmatrix); // B' * C'
2244		- LA.matConcat(origin.toParent, tempmatrix, toParent);
	2742	+
	2743	+//?? LA.matConcat(bone.get(0).fromParent, bone.fromParent, tempmatrix); // B' * C'
	2744	+//?? LA.matConcat(origin.toParent, tempmatrix, toParent);
2245	2745
2246	2746	// matrixToEulerXYZ(toParent, temp);
2247	2747	// matrixToEulerXYZ(bone.get(0).toParent, temp);
..	..	@@ -2275,7 +2775,7 @@
2275	2775	//
2276	2776	// mat4d.get(quat);
2277	2777
2278		- if (true) // hip correction
	2778	+ if (false) // hip correction
2279	2779	{
2280	2780	_t2.setIdentity();
2281	2781
..	..	@@ -2316,13 +2816,13 @@
2316	2816	//????? LA.matConcat(bone.get(0).toParent, bone.toParent, tempmatrix); // B' * C'
2317	2817	// LA.matConcat(origin.fromParent, tempmatrix, fromParent);
2318	2818
2319		- LA.matInvert(toParent, fromParent);
	2819	+//?? LA.matInvert(toParent, fromParent);
2320	2820
2321		- Invariants();
2322		- bone.Invariants();
2323		- bone.get(0).Invariants();
2324		- origin.Invariants();
2325		- origin0.Invariants();
	2821	+ Invariants();
	2822	+ bone.Invariants();
	2823	+ bone.get(0).Invariants();
	2824	+ origin.Invariants();
	2825	+ origin0.Invariants();
2326	2826
2327	2827	// LA.matInvert(fromParent, tempmatrix);
2328	2828	//
..	..	@@ -2343,33 +2843,39 @@
2343	2843	// temp.x = data[0];
2344	2844	// temp.y = data[1];
2345	2845	// temp.z = data[2];
2346		- LA.xformPos(temp, origin0.toParent, temp0);
	2846	+ LA.xformPos(temp, origin0.toParent, temp0); // NU
2347	2847
2348		-// bone.GlobalTransformInv();
2349		-
2350		- temp0.x = bone.globalTransform[3][0];
2351		- temp0.y = bone.globalTransform[3][1];
2352		- temp0.z = bone.globalTransform[3][2];
2353		-
	2848	+// bone.get(0).GlobalTransformInv();
	2849	+//
	2850	+// temp0.x = bone.get(0).globalTransform[3][0];
	2851	+// temp0.y = bone.get(0).globalTransform[3][1];
	2852	+// temp0.z = bone.get(0).globalTransform[3][2];
	2853	+//
2354	2854	// MEGA TEST // june 2014: necessary for precision? otherwise comment works
2355		-fromParent[3][0] = 0;
2356		-fromParent[3][1] = 0;
2357		-fromParent[3][2] = 0;
2358		-LA.matInvert(fromParent, toParent);
	2855	+//fromParent[3][0] = 0;
	2856	+//fromParent[3][1] = 0;
	2857	+//fromParent[3][2] = 0;
	2858	+//LA.matInvert(fromParent, toParent);
2359	2859
2360		- LA.xformPos(temp0, fromParent, temp);
	2860	+ LA.xformPos(temp0, fromParent, temp); // NU
2361	2861	//LA.xformPos(temp, bone.fromParent, temp);
2362	2862	// float[] data;
2363	2863
2364		- pos[0] = temp.x;
2365		- pos[1] = temp.y;
2366		- pos[2] = temp.z;
	2864	+// pos[0] = temp.x;
	2865	+// pos[1] = temp.y;
	2866	+// pos[2] = temp.z;
	2867	+//
	2868	+// temp.x = 1;
	2869	+// temp.y = 0;
	2870	+// temp.z = 0;
	2871	+//
	2872	+// LA.xformDir(temp, bone.get(0).globalTransform, temp);
2367	2873
2368	2874	// CameraPane.selectedpoint.toParent[3][0] = temp0.x;
2369	2875	// CameraPane.selectedpoint.toParent[3][1] = temp0.y;
2370	2876	// CameraPane.selectedpoint.toParent[3][2] = temp0.z;
2371	2877
2372		- int numframes = bvh.animation.getNumFrames();
	2878	+ int numframes = bvh.animation.getNumFrames();
2373	2879
2374	2880	int b = 0;
2375	2881	{
..	..	@@ -2377,12 +2883,12 @@
2377	2883
2378	2884	int dof = 6; /// data.length/numframes;
2379	2885
2380		- // july 2014
2381		- if (dof != data.length/numframes)
2382		- {
2383		- System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (data.length/numframes));
2384		- dof = data.length/numframes;
2385		- }
	2886	+ // july 2014
	2887	+ if (dof != data.length/numframes)
	2888	+ {
	2889	+ System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (data.length/numframes));
	2890	+ dof = data.length/numframes;
	2891	+ }
2386	2892
2387	2893
2388	2894	int theframe = 0; // baseframe;
..	..	@@ -2434,17 +2940,26 @@
2434	2940	// }
2435	2941	// }
2436	2942
2437		- data[f3 + i] += pos[i] - data[frame3 + i];
	2943	+ data[f3 + i] += /pos[i]/ - data[frame3 + i];
2438	2944
2439	2945	// if (f < 3)
2440	2946	// {
2441	2947	// System.out.print(data[f3 + i] + " ");
2442	2948	// }
2443	2949	}
2444		-// if (f < 3)
2445		-// {
2446		-// System.out.println();
2447		-// }
	2950	+
	2951	+ double t1 = data[f3 + 0];
	2952	+ double t2 = data[f3 + 2];
	2953	+
	2954	+ data[f3 + 0] = (float)(cosYt1 - sinYt2);
	2955	+ data[f3 + 2] = (float)(sinYt1 + cosYt2);
	2956	+
	2957	+ for (int i=3/dof/; --i>=0;)
	2958	+ {
	2959	+ data[f3 + i] += pos[i];
	2960	+ }
	2961	+
	2962	+ data[f3 + 4] += angleY - data[frame3 + 4];
2448	2963	}
2449	2964	}
2450	2965
..	..	@@ -2458,186 +2973,17 @@
2458	2973	// temp.y = data[1];
2459	2974	// temp.z = data[2];
2460	2975
2461		- if (fade && currentbones != null && !CameraPane.fullreset)
2462		- {
2463		- Quat4d quatstart = new Quat4d();
2464		- Quat4d quatend = new Quat4d();
2465		- Quat4d quat = new Quat4d();
2466		- Matrix4d mat4d = new Matrix4d();
2467		- double[][] rot = new double[4][4];
2468		-
2469		- for (b=numbones; --b>=1;) // HIP ROTATION: 0;)
2470		- {
2471		- int hipindex = get(0)._index;
2472		-
2473		-// assert(get(0).get(0)._isHip);
2474		-
2475		- // if (b == hipindex)
2476		- // continue;
2477		-
2478		- data = bvh.animation.getBoneData(b);
2479		-
2480		- int dof = 3; // data.length/numframes;
2481		-
2482		- // assert(dof == 3);
2483		-
2484		- int boneframe3 = b*3; // dof;
2485		-
2486		- // System.err.println("Bone #" + b + ": dof = " + dof);
2487		- int fadein = 45; // 120;
2488		-
2489		- // if (b == hipindex)
2490		- // fadein *= 5;
2491		-
2492		- if (fadein > numframes)
2493		- fadein = 0; // numframes;
2494		-
2495		- for (int fi=fadein; --fi>=0;)
2496		- //for (int f=numframes; --f>=0;)
2497		- {
2498		- int f = fi + GetFirstFrame();
2499		-
2500		- int f3 = f*dof;
2501		-
2502		- float k = fi;
2503		-
2504		- k /= fadein - 1;
2505		-
2506		- if (Float.isNaN(k))
2507		- k = 1; // 0/0
2508		-
2509		- k = (float) (-Math.cos(k*Math.PI));
2510		- k += 1;
2511		- k /= 2;
2512		-
2513		- int start = 3;
2514		- int end = 0;
2515		-
2516		- if (dof == 6)
2517		- {
2518		- start = 6;
2519		- end = 3;
2520		- }
2521		-
2522		- if (b == 0)
2523		- {
2524		- _t1.setIdentity();
2525		- _t2.setIdentity();
2526		- _t2.rotZ(currentbones[boneframe3 + start-1-end]);
2527		- _t1.mul(_t2);
2528		- _t2.setIdentity();
2529		- if (b == 0)
2530		- {
2531		- _t2.rotY(currentbones[boneframe3 + start-2-end]);
2532		- _t1.mul(_t2);
2533		- _t2.setIdentity();
2534		- _t2.rotX(currentbones[boneframe3 + start-3-end]);
2535		- }
2536		- else
2537		- {
2538		- _t2.rotX(currentbones[boneframe3 + start-2-end]);
2539		- _t1.mul(_t2);
2540		- _t2.setIdentity();
2541		- _t2.rotY(currentbones[boneframe3 + start-3-end]);
2542		- }
2543		- _t1.mul(_t2);
2544		- _t2.setIdentity();
2545		-
2546		- _t1.get(mat4d);
2547		-
2548		- mat4d.get(quatstart);
2549		-
2550		- _t1.setIdentity();
2551		- _t2.setIdentity();
2552		- _t2.rotZ(data[f3 + start-1]);
2553		- _t1.mul(_t2);
2554		- _t2.setIdentity();
2555		- if (b == 0)
2556		- {
2557		- _t2.rotY(data[f3 + start-2]);
2558		- _t1.mul(_t2);
2559		- _t2.setIdentity();
2560		- _t2.rotX(data[f3 + start-3]);
2561		- }
2562		- else
2563		- {
2564		- _t2.rotX(data[f3 + start-2]);
2565		- _t1.mul(_t2);
2566		- _t2.setIdentity();
2567		- _t2.rotY(data[f3 + start-3]);
2568		- }
2569		-
2570		- _t1.mul(_t2);
2571		- _t2.setIdentity();
2572		-
2573		- _t1.get(mat4d);
2574		-
2575		- mat4d.get(quatend);
2576		-
2577		- double dot = quatstart.x*quatend.x +
2578		- quatstart.y*quatend.y +
2579		- quatstart.z*quatend.z +
2580		- quatstart.w*quatend.w
2581		- ;
2582		-
2583		- if (dot < 0)
2584		- {
2585		- quatend.x *= -1;
2586		- quatend.y *= -1;
2587		- quatend.z *= -1;
2588		- quatend.w *= -1;
2589		- }
2590		-
2591		- // k = 1-k;
2592		-
2593		- quat.x = (1-k)quatstart.x + kquatend.x;
2594		- quat.y = (1-k)quatstart.y + kquatend.y;
2595		- quat.z = (1-k)quatstart.z + kquatend.z;
2596		- quat.w = (1-k)quatstart.w + kquatend.w;
2597		-
2598		- quat.normalize();
2599		-
2600		- mat4d.set(quat);
2601		-
2602		- cJ3D.SetTransform(rot, mat4d);
2603		-
2604		- matrixToEulerXYZ(rot, temp);
2605		-
2606		- data[f3 + start-1] = -(float)temp.z;
2607		-
2608		- if (b == 0)
2609		- {
2610		- data[f3 + start-2] = -(float)temp.y;
2611		- data[f3 + start-3] = -(float)temp.x;
2612		- }
2613		- else
2614		- {
2615		- data[f3 + start-2] = -(float)temp.x;
2616		- data[f3 + start-3] = -(float)temp.y;
2617		- }
2618		- }
2619		- else
2620		- {
2621		- for (int i=start; --i>=end;)
2622		- {
2623		- //data[f3 + i] += pos[i] - data[frame3 + i];
2624		- // data[f3 + i] = kdata[f3+i] + (1-k)
2625		- // currentbones[boneframe3 + i-end];
2626		- data[f3 + i] = (float)CurveAngle(data[f3+i], currentbones[boneframe3 + i-end], 1-k);
2627		- }
2628		- }
2629		-
2630		- }
2631		- }
2632		-
2633		- }
	2976	+// if (fade && currentbones != null && !CameraPane.fullreset)
	2977	+// {
	2978	+// Fade();
	2979	+// }
2634	2980	}
2635		-
	2981	+
2636	2982	if (false) // doesn't change anything bone._isRoot)
2637	2983	{
2638	2984	// Reset orientation
2639	2985	_t2.setIdentity();
2640		-
	2986	+
2641	2987	double tx = bone.toParent[3][0];
2642	2988	double ty = bone.toParent[3][1];
2643	2989	double tz = bone.toParent[3][2];
..	..	@@ -2646,9 +2992,9 @@
2646	2992	temp.y = 0;
2647	2993	temp.z = 0;
2648	2994	LA.xformDir(temp, bone.toParent, temp);
2649		-
	2995	+
2650	2996	double angle = Math.atan2(-temp.z, temp.x);
2651		-
	2997	+
2652	2998	_t2.rotY(angle);
2653	2999	// _t1.setIdentity();
2654	3000	// _t1.rotX(temp.x);
..	..	@@ -2661,52 +3007,16 @@
2661	3007	// _t1.setIdentity();
2662	3008	// _t1.rotZ(temp.z);
2663	3009	// _t2.mul(_t1);
2664		-
	3010	+
2665	3011	cJ3D.ResetTransform(bone, _t2, false);
2666		-
	3012	+
2667	3013	bone.toParent[3][0] = tx;
2668	3014	bone.toParent[3][1] = ty;
2669	3015	bone.toParent[3][2] = tz;
2670	3016	}
2671	3017	}
2672	3018
2673		- if (currentbones == null)
2674		- currentbones = new double[numbones*3];
2675		-
2676		- for (int b=numbones; --b>=1;) // 0;)
2677		- //int b = 0;
2678		- {
2679		- data = bvh.animation.getBoneData(b);
2680		-
2681		- int numframes = bvh.animation.getNumFrames();
2682		-
2683		- int dof = 6; // data.length/numframes;
2684		-
2685		- if (b > 0)
2686		- dof = 3;
2687		-
2688		- //assert(dof == 3);
2689		-
2690		- // System.err.println("Bone #" + b + ": dof = " + dof);
2691		- int theframe = // 1; // baseframe;
2692		- frame;
2693		-
2694		- int frame3 = theframe*dof;
2695		- int bone3 = b*3; // dof;
2696		-
2697		- int start = 3;
2698		- int end = 0;
2699		-
2700		- if (dof == 6)
2701		- {
2702		- start = 6;
2703		- end = 3;
2704		- }
2705		- for (int i=start; --i>=end;)
2706		- {
2707		- currentbones[bone3 + i-end] = data[frame3 + i];
2708		- }
2709		- }
	3019	+ // SetCurrentBones(frame);
2710	3020	}
2711	3021	}
2712	3022
..	..	@@ -2746,7 +3056,7 @@
2746	3056
2747	3057	// assert(obj.name.equals("hip"));
2748	3058
2749		- Object3D rot = hip.get(0);
	3059	+ //Object3D rot = hip.get(0);
2750	3060
2751	3061	// assert(obj.name == null); // .equals("hip#"));
2752	3062
..	..	@@ -2756,8 +3066,8 @@
2756	3066	hip.fromParent = LA.newMatrix();
2757	3067	}
2758	3068
2759		- hip.toParent[3][0] = hip.toParent[3][1] = hip.toParent[3][2] = 0;
2760		- hip.fromParent[3][0] = hip.fromParent[3][1] = hip.fromParent[3][2] = 0;
	3069	+ //hip.toParent[3][0] = hip.toParent[3][1] = hip.toParent[3][2] = 0;
	3070	+ //hip.fromParent[3][0] = hip.fromParent[3][1] = hip.fromParent[3][2] = 0;
2761	3071
2762	3072	if (toParent == null)
2763	3073	{
..	..	@@ -2769,25 +3079,28 @@
2769	3079	LA.matIdentity(fromParent);
2770	3080	// LA.matIdentity(hip.toParent);
2771	3081	// LA.matIdentity(hip.fromParent);
2772		- if (rot.toParent == null)
2773		- {
2774		- // june 2014
2775		- rot.toParent = LA.newMatrix();
2776		- rot.fromParent = LA.newMatrix();
2777		- }
2778		- LA.matIdentity(rot.toParent);
2779		- LA.matIdentity(rot.fromParent);
	3082	+// if (rot.toParent == null)
	3083	+// {
	3084	+// // june 2014
	3085	+// rot.toParent = LA.newMatrix();
	3086	+// rot.fromParent = LA.newMatrix();
	3087	+// }
	3088	+// LA.matIdentity(rot.toParent);
	3089	+// LA.matIdentity(rot.fromParent);
2780	3090
2781	3091	// july 2014
2782	3092	goalx = goalz = 0;
2783	3093	targetx = targetz = 0;
2784	3094
2785		- SetPositionDelta(true, true, true, false);
	3095	+ //SetPositionDelta(true, true, true, false);
	3096	+// LoadData();
	3097	+// Rewind();
2786	3098
2787	3099	baseframe = frame = 0; // T pose. No offset.
2788	3100
2789	3101	// aout 2013 endframe = 0; // june 2013
2790	3102
	3103	+ //ResetZero();
2791	3104	setPose(frame);
2792	3105	}
2793	3106
..	..	@@ -2833,6 +3146,7 @@
2833	3146	if (lastframetest == 0)
2834	3147	lastframetest = bvh.animation.getNumFrames();
2835	3148
	3149	+ // WARNING: RESET DESTROYS EVERYTHING
2836	3150	if (baseframe >= lastframetest) // july 2013 // - GetFirstFrame())
2837	3151	{
2838	3152	System.out.println("MOCAP reset: " + this.GetFileRoot() +
..	..	@@ -2858,7 +3172,10 @@
2858	3172	{
2859	3173	bvh = ((Mocap)support.get(mocapsupport++ % support.Size())).bvh;
2860	3174
2861		- SetPositionDelta(false, true, true, true);
	3175	+ // SetPositionDelta(false, true, true, true);
	3176	+ LoadData();
	3177	+ Rewind();
	3178	+ Fade();
2862	3179	}
2863	3180	catch (Exception e)
2864	3181	{
..	..	@@ -3171,6 +3488,8 @@
3171	3488	//static
3172	3489	transient
3173	3490	double[] currentbones;
	3491	+ double[] currenthip;
	3492	+
3174	3493	transient boolean inbetween;
3175	3494
3176	3495	void SetPositionDelta(boolean reset, boolean rewind, boolean load, boolean fade)
..	..	@@ -3181,38 +3500,9 @@
3181	3500	//assert(dim == numframes*6);
3182	3501
3183	3502	if (load)
3184		- for (int b=numbones; --b>=0;)
3185		- //int b = 0;
3186		- {
3187		- float[] thedata = bvh.animation.getBoneData(b);
3188		-
3189		- int numframes = bvh.animation.getNumFrames();
3190		-
3191		- int dof = 6; // thedata.length/numframes;
3192		-
3193		- if (b > 0)
3194		- dof = 3;
3195		-
3196		- if (dof != thedata.length/numframes)
3197		- {
3198		- System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (thedata.length/numframes));
3199		- dof = thedata.length/numframes;
3200		- }
3201		-
3202		- //for (int f=numframes; --f>=offset;)
3203		- int firstframe = GetFirstFrame();
3204		- for (int f=0; f<numframes-firstframe; f++)
3205		- {
3206		- int f3 = f*dof;
3207		-
3208		- //for (int i=3/dof/; --i>=0;)
3209		- for (int i=dof; --i>=0;)
3210		- {
3211		- //thedata[f3 + i] += thedata[i] - thedata[offset*dof + i];
3212		- thedata[f3 + i] = thedata[f3 + firstframe*dof + i];
3213		- }
3214		- }
3215		- }
	3503	+ {
	3504	+ LoadData();
	3505	+ }
3216	3506
3217	3507	// assert(thedata[0] == 0);
3218	3508	// assert(thedata[1] == 0);
..	..	@@ -3222,11 +3512,11 @@
3222	3512	// assert(thedata[7] == 0);
3223	3513	// assert(thedata[8] == 0);
3224	3514
3225		- Object3D obj = get(0);
	3515	+ Object3D hip = get(0);
3226	3516
3227	3517	// assert(obj.name.equals("hip"));
3228	3518
3229		- Object3D rot = obj.get(0);
	3519	+ Object3D rot = hip.get(0);
3230	3520
3231	3521	// if (toParent == null)
3232	3522	// {
..	..	@@ -3238,7 +3528,7 @@
3238	3528	// // assert(obj.name == null); // .equals("hip#"));
3239	3529
3240	3530	Invariants();
3241		- obj.Invariants();
	3531	+ hip.Invariants();
3242	3532	rot.Invariants();
3243	3533
3244	3534	if (origin == null)
..	..	@@ -3259,16 +3549,16 @@
3259	3549	fromParent = LA.newMatrix();
3260	3550	}
3261	3551
3262		- if (obj.toParent == null) // june 2014
	3552	+ if (hip.toParent == null) // june 2014
3263	3553	{
3264		- obj.toParent = LA.newMatrix();
3265		- obj.fromParent = LA.newMatrix();
	3554	+ hip.toParent = LA.newMatrix();
	3555	+ hip.fromParent = LA.newMatrix();
3266	3556	}
3267	3557
3268		- LA.matConcat(toParent, obj.toParent, origin0.toParent); // AB
	3558	+ LA.matConcat(toParent, hip.toParent, origin0.toParent); // AB
3269	3559	Object3D gp = this.parent;
3270	3560	this.parent = null;
3271		- obj.GlobalTransformInv();
	3561	+ hip.GlobalTransformInv();
3272	3562	this.parent = gp;
3273	3563
3274	3564	if (rot.toParent == null)
..	..	@@ -3291,41 +3581,41 @@
3291	3581	LA.matInvert(origin.toParent, origin.fromParent);
3292	3582
3293	3583	Invariants();
3294		- obj.Invariants();
	3584	+ hip.Invariants();
3295	3585	rot.Invariants();
3296	3586	origin.Invariants();
3297	3587	origin0.Invariants();
3298	3588
3299		- firsttimeafterload = true;
	3589	+ firsttimeafterload = false; // true;
3300	3590
3301	3591	if (rewind)
3302	3592	Rewind();
3303	3593
3304		- setPose(obj, frame, bvh.animation.getBoneData(obj._index), fade);
	3594	+ setPose(hip, frame, bvh.animation.getBoneData(hip._index)); //, fade);
3305	3595
3306		- if (true)
3307		- return;
	3596	+ if (true)
	3597	+ return;
3308	3598
3309	3599	double pos[] = new double[6];
3310	3600
3311	3601	if (!reset)
3312	3602	{
3313		- assert(obj.tx == obj.toParent[3][0]);
3314		- assert(obj.ty == obj.toParent[3][1]);
3315		- assert(obj.tz == obj.toParent[3][2]);
	3603	+ assert(hip.tx == hip.toParent[3][0]);
	3604	+ assert(hip.ty == hip.toParent[3][1]);
	3605	+ assert(hip.tz == hip.toParent[3][2]);
3316	3606
3317		- pos[0] = obj.tx; // obj.toParent[3][0];
3318		- pos[1] = obj.ty; // obj.toParent[3][1];
3319		- pos[2] = obj.tz; // obj.toParent[3][2];
3320		- pos[3] = obj.rz;
3321		- pos[4] = obj.ry;
3322		- pos[5] = obj.rx;
	3607	+ pos[0] = hip.tx; // obj.toParent[3][0];
	3608	+ pos[1] = hip.ty; // obj.toParent[3][1];
	3609	+ pos[2] = hip.tz; // obj.toParent[3][2];
	3610	+ pos[3] = hip.rz;
	3611	+ pos[4] = hip.ry;
	3612	+ pos[5] = hip.rx;
3323	3613	}
3324	3614
3325	3615	cVector temp = new cVector();
3326		- temp.x = obj.globalTransform[3][0];
3327		- temp.y = obj.globalTransform[3][1];
3328		- temp.z = obj.globalTransform[3][2];
	3616	+ temp.x = hip.globalTransform[3][0];
	3617	+ temp.y = hip.globalTransform[3][1];
	3618	+ temp.z = hip.globalTransform[3][2];
3329	3619	// LA.xformPos(temp, origin0.toParent, temp);
3330	3620	// System.err.println("POS0 = " + temp.x + ", " + temp.y + ", " + temp.z + ";");
3331	3621
..	..	@@ -3409,7 +3699,7 @@
3409	3699	}
3410	3700
3411	3701	// Rewind();
3412		- setPose(obj, frame, bvh.animation.getBoneData(obj._index), true);
	3702	+ setPose(hip, frame, bvh.animation.getBoneData(hip._index)); // , true);
3413	3703	}
3414	3704
3415	3705	void Rewind()