Normand Briere
2018-07-01 655810d1c4e710e7c85772b8dde96772dbcf274b
Mocap.java
....@@ -26,6 +26,450 @@
2626 public class Mocap extends Object3D
2727 {
2828 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] = k*data[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 + k*quatend.x;
185
+// quat.y = (1-k)*quatstart.y + k*quatend.y;
186
+// quat.z = (1-k)*quatstart.z + k*quatend.z;
187
+// quat.w = (1-k)*quatstart.w + k*quatend.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] = k*data[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
+ // AT*AR = A'T*BT*A'R*BR
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
+ }
29473
30474 void WriteTo(java.io.Writer writer) throws Exception
31475 {
....@@ -705,7 +1149,7 @@
7051149 // skip first node: translation
7061150 // Object3D b = input.get(0);
7071151
708
- setPose(input, f, data.getBoneData(input._index), false); //, null);
1152
+ setPose(input, f, data.getBoneData(input._index)); //, false); //, null);
7091153 }
7101154
7111155 //mocap.figure.Bone[] children = b.getChildren();
....@@ -921,7 +1365,7 @@
9211365 //groundz += sourcenode.parent.toParent[3][2] - posz;
9221366 }
9231367
924
- boolean footcontact = CameraPane.FOOTCONTACT; // false;
1368
+ boolean footcontact = false; // CameraPane.FOOTCONTACT; // false;
9251369
9261370 if (footcontact && ground != -123456)
9271371 {
....@@ -1220,6 +1664,7 @@
12201664 src.x = 0;
12211665 src.y = 0;
12221666 src.z = 0;
1667
+ LA.xformPos(src, get(0).get(0).toParent, src);
12231668 LA.xformPos(src, get(0).toParent, src);
12241669 LA.xformPos(src, toParent, src);
12251670 // LA.xformPos(src, rotsourcenode.toParent, src);
....@@ -1563,6 +2008,7 @@
15632008 toParent = LA.newMatrix();
15642009 fromParent = LA.newMatrix();
15652010 }
2011
+
15662012 LA.matConcat(toParent, rotsourcenode.toParent, toParent);
15672013 LA.matInvert(toParent, fromParent);
15682014
....@@ -1698,7 +2144,7 @@
16982144 }
16992145
17002146 if (!smoothed)
1701
- for (int i=4; --i>=0;)
2147
+ for (int i=0; --i>=0;)
17022148 SmoothAnimData(); // much reduces shakiness
17032149
17042150 smoothed = true;
....@@ -1780,12 +2226,26 @@
17802226 if (!live) // aout 2013
17812227 return;
17822228
1783
- cVector centroid = new cVector();
1784
- cVector floor = new cVector();
1785
-
1786
- 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
+ }
17872239
1788
- 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);
17892249 // if (!success)
17902250 // {
17912251 // //assert(success);
....@@ -1793,16 +2253,6 @@
17932253 // System.out.println("No centroid for " + sourcenode);
17942254 // }
17952255
1796
- //bvh.skeleton.setPose(f, bvh.animation.getBoneData(bvh.skeleton.getIndex()), null);
1797
- try
1798
- {
1799
- SetPose(GetSkeleton(), bvh.animation, f);
1800
- }
1801
- catch (Exception e)
1802
- {
1803
- e.printStackTrace();
1804
- }
1805
-
18062256 if (sourcenode.parent == null && sourcenode.fileparent == null)
18072257 {
18082258 // No GFD
....@@ -2050,7 +2500,7 @@
20502500 * @param frame
20512501 * Frame number
20522502 */
2053
- 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)
20542504 {
20552505 if (!live) // aout 2013
20562506 return;
....@@ -2058,6 +2508,33 @@
20582508 cVector temp = new cVector();
20592509 cVector temp0 = new cVector();
20602510 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
+ }
20612538
20622539 // Object3D obj = get(0);
20632540 // Object3D gp = this.parent;
....@@ -2187,15 +2664,15 @@
21872664 // set rotation
21882665 //if (bone._rotationEnabled)
21892666 if (//frame > 0 &&
2190
- !bone.skipmocap &&
2191
- !bone.name.contains("head") &&
2667
+ !bone.skipmocap //&&
2668
+ //!bone.name.contains("head") &&
21922669 // !bone.name.contains("rFoot") &&
21932670 // !bone.name.contains("lFoot") &&
2194
- !bone.name.contains("Pinky") &&
2195
- !bone.name.contains("Ring") &&
2196
- !bone.name.contains("Mid") &&
2197
- !bone.name.contains("Index") &&
2198
- !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")
21992676 )
22002677 {
22012678 //_rotTG.setTransform(_t1);
....@@ -2228,15 +2705,17 @@
22282705 // CROWD
22292706 //cJ3D.GetTranslation(bone, _trans0);
22302707 //_trans.add(_trans0);
2231
- _transTF.setIdentity();
2708
+ //_transTF.setIdentity();
2709
+ _transTF.set(_t1);
22322710 _transTF.setTranslation(_trans);
22332711 // System.err.println("Translate: " + _transTF);
22342712 //_transTG.setTransform(_transTF);
2235
- 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
22362715 }
22372716 }
22382717
2239
- if (bone._isRoot && firsttimeafterload && frame > 0)
2718
+ if (false) // bone._isRoot && firsttimeafterload && frame > 0)
22402719 {
22412720 assert(bone == get(0));
22422721
....@@ -2252,17 +2731,17 @@
22522731 // LA.matConcat(originmatrixinv, tempmatrixinv, fromParent); // ABC * (B'*C')^-1
22532732 //
22542733
2255
- Invariants();
2256
- bone.Invariants();
2257
- bone.get(0).Invariants();
2258
- // if (origin == null)
2259
- // origin = null;
2260
- origin.Invariants();
2261
- 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();
22622741
2263
-
2264
- LA.matConcat(bone.get(0).fromParent, bone.fromParent, tempmatrix); // B' * C'
2265
- 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);
22662745
22672746 // matrixToEulerXYZ(toParent, temp);
22682747 // matrixToEulerXYZ(bone.get(0).toParent, temp);
....@@ -2296,7 +2775,7 @@
22962775 //
22972776 // mat4d.get(quat);
22982777
2299
- if (true) // hip correction
2778
+ if (false) // hip correction
23002779 {
23012780 _t2.setIdentity();
23022781
....@@ -2337,13 +2816,13 @@
23372816 //????? LA.matConcat(bone.get(0).toParent, bone.toParent, tempmatrix); // B' * C'
23382817 // LA.matConcat(origin.fromParent, tempmatrix, fromParent);
23392818
2340
- LA.matInvert(toParent, fromParent);
2819
+//?? LA.matInvert(toParent, fromParent);
23412820
2342
- Invariants();
2343
- bone.Invariants();
2344
- bone.get(0).Invariants();
2345
- origin.Invariants();
2346
- origin0.Invariants();
2821
+ Invariants();
2822
+ bone.Invariants();
2823
+ bone.get(0).Invariants();
2824
+ origin.Invariants();
2825
+ origin0.Invariants();
23472826
23482827 // LA.matInvert(fromParent, tempmatrix);
23492828 //
....@@ -2364,33 +2843,39 @@
23642843 // temp.x = data[0];
23652844 // temp.y = data[1];
23662845 // temp.z = data[2];
2367
- LA.xformPos(temp, origin0.toParent, temp0);
2846
+ LA.xformPos(temp, origin0.toParent, temp0); // NU
23682847
2369
-// bone.GlobalTransformInv();
2370
-
2371
- temp0.x = bone.globalTransform[3][0];
2372
- temp0.y = bone.globalTransform[3][1];
2373
- temp0.z = bone.globalTransform[3][2];
2374
-
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
+//
23752854 // MEGA TEST // june 2014: necessary for precision? otherwise comment works
2376
-fromParent[3][0] = 0;
2377
-fromParent[3][1] = 0;
2378
-fromParent[3][2] = 0;
2379
-LA.matInvert(fromParent, toParent);
2855
+//fromParent[3][0] = 0;
2856
+//fromParent[3][1] = 0;
2857
+//fromParent[3][2] = 0;
2858
+//LA.matInvert(fromParent, toParent);
23802859
2381
- LA.xformPos(temp0, fromParent, temp);
2860
+ LA.xformPos(temp0, fromParent, temp); // NU
23822861 //LA.xformPos(temp, bone.fromParent, temp);
23832862 // float[] data;
23842863
2385
- pos[0] = temp.x;
2386
- pos[1] = temp.y;
2387
- 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);
23882873
23892874 // CameraPane.selectedpoint.toParent[3][0] = temp0.x;
23902875 // CameraPane.selectedpoint.toParent[3][1] = temp0.y;
23912876 // CameraPane.selectedpoint.toParent[3][2] = temp0.z;
23922877
2393
- int numframes = bvh.animation.getNumFrames();
2878
+ int numframes = bvh.animation.getNumFrames();
23942879
23952880 int b = 0;
23962881 {
....@@ -2398,12 +2883,12 @@
23982883
23992884 int dof = 6; /// data.length/numframes;
24002885
2401
- // july 2014
2402
- if (dof != data.length/numframes)
2403
- {
2404
- System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (data.length/numframes));
2405
- dof = data.length/numframes;
2406
- }
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
+ }
24072892
24082893
24092894 int theframe = 0; // baseframe;
....@@ -2455,17 +2940,26 @@
24552940 // }
24562941 // }
24572942
2458
- data[f3 + i] += pos[i] - data[frame3 + i];
2943
+ data[f3 + i] += /*pos[i]*/ - data[frame3 + i];
24592944
24602945 // if (f < 3)
24612946 // {
24622947 // System.out.print(data[f3 + i] + " ");
24632948 // }
24642949 }
2465
-// if (f < 3)
2466
-// {
2467
-// System.out.println();
2468
-// }
2950
+
2951
+ double t1 = data[f3 + 0];
2952
+ double t2 = data[f3 + 2];
2953
+
2954
+ data[f3 + 0] = (float)(cosY*t1 - sinY*t2);
2955
+ data[f3 + 2] = (float)(sinY*t1 + cosY*t2);
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];
24692963 }
24702964 }
24712965
....@@ -2479,186 +2973,17 @@
24792973 // temp.y = data[1];
24802974 // temp.z = data[2];
24812975
2482
- if (fade && currentbones != null && !CameraPane.fullreset)
2483
- {
2484
- Quat4d quatstart = new Quat4d();
2485
- Quat4d quatend = new Quat4d();
2486
- Quat4d quat = new Quat4d();
2487
- Matrix4d mat4d = new Matrix4d();
2488
- double[][] rot = new double[4][4];
2489
-
2490
- for (b=numbones; --b>=1;) // HIP ROTATION: 0;)
2491
- {
2492
- int hipindex = get(0)._index;
2493
-
2494
-// assert(get(0).get(0)._isHip);
2495
-
2496
- // if (b == hipindex)
2497
- // continue;
2498
-
2499
- data = bvh.animation.getBoneData(b);
2500
-
2501
- int dof = 3; // data.length/numframes;
2502
-
2503
- // assert(dof == 3);
2504
-
2505
- int boneframe3 = b*3; // dof;
2506
-
2507
- // System.err.println("Bone #" + b + ": dof = " + dof);
2508
- int fadein = 45; // 120;
2509
-
2510
- // if (b == hipindex)
2511
- // fadein *= 5;
2512
-
2513
- if (fadein > numframes)
2514
- fadein = 0; // numframes;
2515
-
2516
- for (int fi=fadein; --fi>=0;)
2517
- //for (int f=numframes; --f>=0;)
2518
- {
2519
- int f = fi + GetFirstFrame();
2520
-
2521
- int f3 = f*dof;
2522
-
2523
- float k = fi;
2524
-
2525
- k /= fadein - 1;
2526
-
2527
- if (Float.isNaN(k))
2528
- k = 1; // 0/0
2529
-
2530
- k = (float) (-Math.cos(k*Math.PI));
2531
- k += 1;
2532
- k /= 2;
2533
-
2534
- int start = 3;
2535
- int end = 0;
2536
-
2537
- if (dof == 6)
2538
- {
2539
- start = 6;
2540
- end = 3;
2541
- }
2542
-
2543
- if (b == 0)
2544
- {
2545
- _t1.setIdentity();
2546
- _t2.setIdentity();
2547
- _t2.rotZ(currentbones[boneframe3 + start-1-end]);
2548
- _t1.mul(_t2);
2549
- _t2.setIdentity();
2550
- if (b == 0)
2551
- {
2552
- _t2.rotY(currentbones[boneframe3 + start-2-end]);
2553
- _t1.mul(_t2);
2554
- _t2.setIdentity();
2555
- _t2.rotX(currentbones[boneframe3 + start-3-end]);
2556
- }
2557
- else
2558
- {
2559
- _t2.rotX(currentbones[boneframe3 + start-2-end]);
2560
- _t1.mul(_t2);
2561
- _t2.setIdentity();
2562
- _t2.rotY(currentbones[boneframe3 + start-3-end]);
2563
- }
2564
- _t1.mul(_t2);
2565
- _t2.setIdentity();
2566
-
2567
- _t1.get(mat4d);
2568
-
2569
- mat4d.get(quatstart);
2570
-
2571
- _t1.setIdentity();
2572
- _t2.setIdentity();
2573
- _t2.rotZ(data[f3 + start-1]);
2574
- _t1.mul(_t2);
2575
- _t2.setIdentity();
2576
- if (b == 0)
2577
- {
2578
- _t2.rotY(data[f3 + start-2]);
2579
- _t1.mul(_t2);
2580
- _t2.setIdentity();
2581
- _t2.rotX(data[f3 + start-3]);
2582
- }
2583
- else
2584
- {
2585
- _t2.rotX(data[f3 + start-2]);
2586
- _t1.mul(_t2);
2587
- _t2.setIdentity();
2588
- _t2.rotY(data[f3 + start-3]);
2589
- }
2590
-
2591
- _t1.mul(_t2);
2592
- _t2.setIdentity();
2593
-
2594
- _t1.get(mat4d);
2595
-
2596
- mat4d.get(quatend);
2597
-
2598
- double dot = quatstart.x*quatend.x +
2599
- quatstart.y*quatend.y +
2600
- quatstart.z*quatend.z +
2601
- quatstart.w*quatend.w
2602
- ;
2603
-
2604
- if (dot < 0)
2605
- {
2606
- quatend.x *= -1;
2607
- quatend.y *= -1;
2608
- quatend.z *= -1;
2609
- quatend.w *= -1;
2610
- }
2611
-
2612
- // k = 1-k;
2613
-
2614
- quat.x = (1-k)*quatstart.x + k*quatend.x;
2615
- quat.y = (1-k)*quatstart.y + k*quatend.y;
2616
- quat.z = (1-k)*quatstart.z + k*quatend.z;
2617
- quat.w = (1-k)*quatstart.w + k*quatend.w;
2618
-
2619
- quat.normalize();
2620
-
2621
- mat4d.set(quat);
2622
-
2623
- cJ3D.SetTransform(rot, mat4d);
2624
-
2625
- matrixToEulerXYZ(rot, temp);
2626
-
2627
- data[f3 + start-1] = -(float)temp.z;
2628
-
2629
- if (b == 0)
2630
- {
2631
- data[f3 + start-2] = -(float)temp.y;
2632
- data[f3 + start-3] = -(float)temp.x;
2633
- }
2634
- else
2635
- {
2636
- data[f3 + start-2] = -(float)temp.x;
2637
- data[f3 + start-3] = -(float)temp.y;
2638
- }
2639
- }
2640
- else
2641
- {
2642
- for (int i=start; --i>=end;)
2643
- {
2644
- //data[f3 + i] += pos[i] - data[frame3 + i];
2645
- // data[f3 + i] = k*data[f3+i] + (1-k)*
2646
- // currentbones[boneframe3 + i-end];
2647
- data[f3 + i] = (float)CurveAngle(data[f3+i], currentbones[boneframe3 + i-end], 1-k);
2648
- }
2649
- }
2650
-
2651
- }
2652
- }
2653
-
2654
- }
2976
+// if (fade && currentbones != null && !CameraPane.fullreset)
2977
+// {
2978
+// Fade();
2979
+// }
26552980 }
2656
-
2981
+
26572982 if (false) // doesn't change anything bone._isRoot)
26582983 {
26592984 // Reset orientation
26602985 _t2.setIdentity();
2661
-
2986
+
26622987 double tx = bone.toParent[3][0];
26632988 double ty = bone.toParent[3][1];
26642989 double tz = bone.toParent[3][2];
....@@ -2667,9 +2992,9 @@
26672992 temp.y = 0;
26682993 temp.z = 0;
26692994 LA.xformDir(temp, bone.toParent, temp);
2670
-
2995
+
26712996 double angle = Math.atan2(-temp.z, temp.x);
2672
-
2997
+
26732998 _t2.rotY(angle);
26742999 // _t1.setIdentity();
26753000 // _t1.rotX(temp.x);
....@@ -2682,52 +3007,16 @@
26823007 // _t1.setIdentity();
26833008 // _t1.rotZ(temp.z);
26843009 // _t2.mul(_t1);
2685
-
3010
+
26863011 cJ3D.ResetTransform(bone, _t2, false);
2687
-
3012
+
26883013 bone.toParent[3][0] = tx;
26893014 bone.toParent[3][1] = ty;
26903015 bone.toParent[3][2] = tz;
26913016 }
26923017 }
26933018
2694
- if (currentbones == null)
2695
- currentbones = new double[numbones*3];
2696
-
2697
- for (int b=numbones; --b>=1;) // 0;)
2698
- //int b = 0;
2699
- {
2700
- data = bvh.animation.getBoneData(b);
2701
-
2702
- int numframes = bvh.animation.getNumFrames();
2703
-
2704
- int dof = 6; // data.length/numframes;
2705
-
2706
- if (b > 0)
2707
- dof = 3;
2708
-
2709
- //assert(dof == 3);
2710
-
2711
- // System.err.println("Bone #" + b + ": dof = " + dof);
2712
- int theframe = // 1; // baseframe;
2713
- frame;
2714
-
2715
- int frame3 = theframe*dof;
2716
- int bone3 = b*3; // dof;
2717
-
2718
- int start = 3;
2719
- int end = 0;
2720
-
2721
- if (dof == 6)
2722
- {
2723
- start = 6;
2724
- end = 3;
2725
- }
2726
- for (int i=start; --i>=end;)
2727
- {
2728
- currentbones[bone3 + i-end] = data[frame3 + i];
2729
- }
2730
- }
3019
+ // SetCurrentBones(frame);
27313020 }
27323021 }
27333022
....@@ -2767,7 +3056,7 @@
27673056
27683057 // assert(obj.name.equals("hip"));
27693058
2770
- Object3D rot = hip.get(0);
3059
+ //Object3D rot = hip.get(0);
27713060
27723061 // assert(obj.name == null); // .equals("hip#"));
27733062
....@@ -2777,8 +3066,8 @@
27773066 hip.fromParent = LA.newMatrix();
27783067 }
27793068
2780
- hip.toParent[3][0] = hip.toParent[3][1] = hip.toParent[3][2] = 0;
2781
- 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;
27823071
27833072 if (toParent == null)
27843073 {
....@@ -2790,25 +3079,28 @@
27903079 LA.matIdentity(fromParent);
27913080 // LA.matIdentity(hip.toParent);
27923081 // LA.matIdentity(hip.fromParent);
2793
- if (rot.toParent == null)
2794
- {
2795
- // june 2014
2796
- rot.toParent = LA.newMatrix();
2797
- rot.fromParent = LA.newMatrix();
2798
- }
2799
- LA.matIdentity(rot.toParent);
2800
- 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);
28013090
28023091 // july 2014
28033092 goalx = goalz = 0;
28043093 targetx = targetz = 0;
28053094
2806
- SetPositionDelta(true, true, true, false);
3095
+ //SetPositionDelta(true, true, true, false);
3096
+// LoadData();
3097
+// Rewind();
28073098
28083099 baseframe = frame = 0; // T pose. No offset.
28093100
28103101 // aout 2013 endframe = 0; // june 2013
28113102
3103
+ //ResetZero();
28123104 setPose(frame);
28133105 }
28143106
....@@ -2854,6 +3146,7 @@
28543146 if (lastframetest == 0)
28553147 lastframetest = bvh.animation.getNumFrames();
28563148
3149
+ // WARNING: RESET DESTROYS EVERYTHING
28573150 if (baseframe >= lastframetest) // july 2013 // - GetFirstFrame())
28583151 {
28593152 System.out.println("MOCAP reset: " + this.GetFileRoot() +
....@@ -2879,7 +3172,10 @@
28793172 {
28803173 bvh = ((Mocap)support.get(mocapsupport++ % support.Size())).bvh;
28813174
2882
- SetPositionDelta(false, true, true, true);
3175
+ // SetPositionDelta(false, true, true, true);
3176
+ LoadData();
3177
+ Rewind();
3178
+ Fade();
28833179 }
28843180 catch (Exception e)
28853181 {
....@@ -3192,6 +3488,8 @@
31923488 //static
31933489 transient
31943490 double[] currentbones;
3491
+ double[] currenthip;
3492
+
31953493 transient boolean inbetween;
31963494
31973495 void SetPositionDelta(boolean reset, boolean rewind, boolean load, boolean fade)
....@@ -3202,38 +3500,9 @@
32023500 //assert(dim == numframes*6);
32033501
32043502 if (load)
3205
- for (int b=numbones; --b>=0;)
3206
- //int b = 0;
3207
- {
3208
- float[] thedata = bvh.animation.getBoneData(b);
3209
-
3210
- int numframes = bvh.animation.getNumFrames();
3211
-
3212
- int dof = 6; // thedata.length/numframes;
3213
-
3214
- if (b > 0)
3215
- dof = 3;
3216
-
3217
- if (dof != thedata.length/numframes)
3218
- {
3219
- System.err.println("Bone #" + b + ": dof = " + dof + " ---> " + (thedata.length/numframes));
3220
- dof = thedata.length/numframes;
3221
- }
3222
-
3223
- //for (int f=numframes; --f>=offset;)
3224
- int firstframe = GetFirstFrame();
3225
- for (int f=0; f<numframes-firstframe; f++)
3226
- {
3227
- int f3 = f*dof;
3228
-
3229
- //for (int i=3/*dof*/; --i>=0;)
3230
- for (int i=dof; --i>=0;)
3231
- {
3232
- //thedata[f3 + i] += thedata[i] - thedata[offset*dof + i];
3233
- thedata[f3 + i] = thedata[f3 + firstframe*dof + i];
3234
- }
3235
- }
3236
- }
3503
+ {
3504
+ LoadData();
3505
+ }
32373506
32383507 // assert(thedata[0] == 0);
32393508 // assert(thedata[1] == 0);
....@@ -3243,11 +3512,11 @@
32433512 // assert(thedata[7] == 0);
32443513 // assert(thedata[8] == 0);
32453514
3246
- Object3D obj = get(0);
3515
+ Object3D hip = get(0);
32473516
32483517 // assert(obj.name.equals("hip"));
32493518
3250
- Object3D rot = obj.get(0);
3519
+ Object3D rot = hip.get(0);
32513520
32523521 // if (toParent == null)
32533522 // {
....@@ -3259,7 +3528,7 @@
32593528 // // assert(obj.name == null); // .equals("hip#"));
32603529
32613530 Invariants();
3262
- obj.Invariants();
3531
+ hip.Invariants();
32633532 rot.Invariants();
32643533
32653534 if (origin == null)
....@@ -3280,16 +3549,16 @@
32803549 fromParent = LA.newMatrix();
32813550 }
32823551
3283
- if (obj.toParent == null) // june 2014
3552
+ if (hip.toParent == null) // june 2014
32843553 {
3285
- obj.toParent = LA.newMatrix();
3286
- obj.fromParent = LA.newMatrix();
3554
+ hip.toParent = LA.newMatrix();
3555
+ hip.fromParent = LA.newMatrix();
32873556 }
32883557
3289
- LA.matConcat(toParent, obj.toParent, origin0.toParent); // AB
3558
+ LA.matConcat(toParent, hip.toParent, origin0.toParent); // AB
32903559 Object3D gp = this.parent;
32913560 this.parent = null;
3292
- obj.GlobalTransformInv();
3561
+ hip.GlobalTransformInv();
32933562 this.parent = gp;
32943563
32953564 if (rot.toParent == null)
....@@ -3312,41 +3581,41 @@
33123581 LA.matInvert(origin.toParent, origin.fromParent);
33133582
33143583 Invariants();
3315
- obj.Invariants();
3584
+ hip.Invariants();
33163585 rot.Invariants();
33173586 origin.Invariants();
33183587 origin0.Invariants();
33193588
3320
- firsttimeafterload = true;
3589
+ firsttimeafterload = false; // true;
33213590
33223591 if (rewind)
33233592 Rewind();
33243593
3325
- setPose(obj, frame, bvh.animation.getBoneData(obj._index), fade);
3594
+ setPose(hip, frame, bvh.animation.getBoneData(hip._index)); //, fade);
33263595
3327
- if (true)
3328
- return;
3596
+ if (true)
3597
+ return;
33293598
33303599 double pos[] = new double[6];
33313600
33323601 if (!reset)
33333602 {
3334
- assert(obj.tx == obj.toParent[3][0]);
3335
- assert(obj.ty == obj.toParent[3][1]);
3336
- 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]);
33373606
3338
- pos[0] = obj.tx; // obj.toParent[3][0];
3339
- pos[1] = obj.ty; // obj.toParent[3][1];
3340
- pos[2] = obj.tz; // obj.toParent[3][2];
3341
- pos[3] = obj.rz;
3342
- pos[4] = obj.ry;
3343
- 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;
33443613 }
33453614
33463615 cVector temp = new cVector();
3347
- temp.x = obj.globalTransform[3][0];
3348
- temp.y = obj.globalTransform[3][1];
3349
- 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];
33503619 // LA.xformPos(temp, origin0.toParent, temp);
33513620 // System.err.println("POS0 = " + temp.x + ", " + temp.y + ", " + temp.z + ";");
33523621
....@@ -3430,7 +3699,7 @@
34303699 }
34313700
34323701 // Rewind();
3433
- setPose(obj, frame, bvh.animation.getBoneData(obj._index), true);
3702
+ setPose(hip, frame, bvh.animation.getBoneData(hip._index)); // , true);
34343703 }
34353704
34363705 void Rewind()