Revision 6a2ebb18
Added by Leszek Koltunski over 7 years ago
| src/main/java/org/distorted/library/type/Dynamic.java | ||
|---|---|---|
| 46 | 46 |
// we have the solution: X(t) = at^3 + bt^2 + ct + d where |
| 47 | 47 |
// a = 2*P[i](x) + w[i](x) - 2*P[i+1](x) + w[i+1](x) |
| 48 | 48 |
// b = -3*P[i](x) - 2*w[i](x) + 3*P[i+1](x) - w[i+1](x) |
| 49 |
// c = w[i](x)<br>
|
|
| 49 |
// c = w[i](x) |
|
| 50 | 50 |
// d = P[i](x) |
| 51 | 51 |
// |
| 52 | 52 |
// and similarly Y(t) and Z(t). |
| ... | ... | |
| 66 | 66 |
*/ |
| 67 | 67 |
public static final int MODE_JUMP = 2; |
| 68 | 68 |
|
| 69 |
protected static Random mRnd = new Random(); |
|
| 70 |
|
|
| 71 |
protected static final int NUM_NOISE = 5; // used iff mNoise>0.0. Number of intermediary points between each pair of adjacent vectors |
|
| 72 |
// where we randomize noise factors to make the way between the two vectors not so smooth. |
|
| 73 |
|
|
| 74 | 69 |
protected int mDimension; |
| 75 | 70 |
protected int numPoints; |
| 76 | 71 |
protected int mVecCurr; |
| ... | ... | |
| 103 | 98 |
protected float[] mFactor; |
| 104 | 99 |
protected float[] mNoise; |
| 105 | 100 |
protected float[][] baseV; |
| 106 |
private float[] buf; |
|
| 107 |
private float[] old; |
|
| 108 | 101 |
|
| 109 | 102 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
| 110 | 103 |
// the coefficients of the X(t), Y(t) and Z(t) polynomials: X(t) = ax*T^3 + bx*T^2 + cx*t + dx etc. |
| ... | ... | |
| 135 | 128 |
protected Vector<VectorCache> vc; |
| 136 | 129 |
protected VectorCache tmp1, tmp2; |
| 137 | 130 |
|
| 131 |
private float[] buf; |
|
| 132 |
private float[] old; |
|
| 133 |
private static Random mRnd = new Random(); |
|
| 134 |
private static final int NUM_NOISE = 5; // used iff mNoise>0.0. Number of intermediary points between each pair of adjacent vectors |
|
| 135 |
// where we randomize noise factors to make the way between the two vectors not so smooth. |
|
| 136 |
|
|
| 137 |
//private int lastNon; |
|
| 138 |
|
|
| 138 | 139 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
| 139 | 140 |
// hide this from Javadoc |
| 140 | 141 |
|
| ... | ... | |
| 270 | 271 |
|
| 271 | 272 |
private void checkBase() |
| 272 | 273 |
{
|
| 273 |
float tmp; |
|
| 274 |
float tmp, cosA; |
|
| 275 |
float[] len= new float[mDimension]; |
|
| 276 |
boolean error=false; |
|
| 277 |
|
|
| 278 |
for(int i=0; i<mDimension; i++) |
|
| 279 |
{
|
|
| 280 |
len[i] = 0.0f; |
|
| 281 |
|
|
| 282 |
for(int k=0; k<mDimension; k++) |
|
| 283 |
{
|
|
| 284 |
len[i] += baseV[i][k]*baseV[i][k]; |
|
| 285 |
} |
|
| 286 |
|
|
| 287 |
if( len[i] == 0.0f || len[0]/len[i] < 0.95f || len[0]/len[i]>1.05f ) |
|
| 288 |
{
|
|
| 289 |
android.util.Log.e("dynamic", "length of vector "+i+" : "+Math.sqrt(len[i]));
|
|
| 290 |
error = true; |
|
| 291 |
} |
|
| 292 |
} |
|
| 274 | 293 |
|
| 275 | 294 |
for(int i=0; i<mDimension; i++) |
| 276 | 295 |
for(int j=i+1; j<mDimension; j++) |
| ... | ... | |
| 282 | 301 |
tmp += baseV[i][k]*baseV[j][k]; |
| 283 | 302 |
} |
| 284 | 303 |
|
| 285 |
android.util.Log.e("dynamic", "vectors "+i+" and "+j+" : "+tmp);
|
|
| 286 |
} |
|
| 287 |
|
|
| 288 |
for(int i=0; i<mDimension; i++) |
|
| 289 |
{
|
|
| 290 |
tmp = 0.0f; |
|
| 304 |
cosA = ( (len[i]==0.0f || len[j]==0.0f) ? 0.0f : tmp/(float)Math.sqrt(len[i]*len[j])); |
|
| 291 | 305 |
|
| 292 |
for(int k=0; k<mDimension; k++) |
|
| 293 |
{
|
|
| 294 |
tmp += baseV[i][k]*baseV[i][k]; |
|
| 306 |
if( cosA > 0.05f || cosA < -0.05f ) |
|
| 307 |
{
|
|
| 308 |
android.util.Log.e("dynamic", "cos angle between vectors "+i+" and "+j+" : "+cosA);
|
|
| 309 |
error = true; |
|
| 310 |
} |
|
| 295 | 311 |
} |
| 296 | 312 |
|
| 297 |
android.util.Log.e("dynamic", "length of vector "+i+" : "+Math.sqrt(tmp));
|
|
| 298 |
} |
|
| 313 |
if( error ) printBase("");
|
|
| 299 | 314 |
} |
| 300 | 315 |
|
| 301 | 316 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
| ... | ... | |
| 393 | 408 |
computeOrthonormalBase(); |
| 394 | 409 |
} |
| 395 | 410 |
|
| 411 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
|
| 412 |
// debugging |
|
| 413 |
/* |
|
| 414 |
protected void computeOrthonormalBaseMoreDebug(float time,VectorCache vc) |
|
| 415 |
{
|
|
| 416 |
for(int i=0; i<mDimension; i++) |
|
| 417 |
{
|
|
| 418 |
baseV[0][i] = (3*vc.a[i]*time+2*vc.b[i])*time+vc.c[i]; // first derivative, i.e. velocity vector |
|
| 419 |
baseV[1][i] = 6*vc.a[i]*time+2*vc.b[i]; // second derivative,i.e. acceleration vector |
|
| 420 |
} |
|
| 421 |
|
|
| 422 |
float av=0.0f, vv=0.0f; |
|
| 423 |
|
|
| 424 |
android.util.Log.e("dyn3D", " ==> velocity ("+baseV[0][0]+","+baseV[0][1]+","+baseV[0][2]+")");
|
|
| 425 |
android.util.Log.e("dyn3D", " ==> acceleration ("+baseV[1][0]+","+baseV[1][1]+","+baseV[1][2]+")");
|
|
| 426 |
|
|
| 427 |
for(int k=0; k<mDimension; k++) |
|
| 428 |
{
|
|
| 429 |
vv += baseV[0][k]*baseV[0][k]; |
|
| 430 |
av += baseV[1][k]*baseV[0][k]; |
|
| 431 |
} |
|
| 432 |
|
|
| 433 |
android.util.Log.e("dyn3D", " ==> av: "+av+" vv="+vv);
|
|
| 434 |
|
|
| 435 |
av /= vv; |
|
| 436 |
|
|
| 437 |
for(int k=0;k<mDimension; k++) |
|
| 438 |
{
|
|
| 439 |
baseV[1][k] -= av*baseV[0][k]; |
|
| 440 |
} |
|
| 441 |
|
|
| 442 |
android.util.Log.e("dyn3D", " ==> second base ("+baseV[1][0]+","+baseV[1][1]+","+baseV[1][2]+")");
|
|
| 443 |
} |
|
| 444 |
*/ |
|
| 396 | 445 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
| 397 | 446 |
// helper function in case we are interpolating through more than 2 points |
| 398 | 447 |
|
| ... | ... | |
| 400 | 449 |
{
|
| 401 | 450 |
for(int i=0; i<mDimension; i++) |
| 402 | 451 |
{
|
| 403 |
baseV[0][i] = (3*vc.a[i]*time+2*vc.b[i])*time+vc.c[i]; |
|
| 404 |
baseV[1][i] = 6*vc.a[i]*time+2*vc.b[i]; |
|
| 452 |
baseV[0][i] = (3*vc.a[i]*time+2*vc.b[i])*time+vc.c[i]; // first derivative, i.e. velocity vector |
|
| 453 |
old[i] = baseV[1][i]; |
|
| 454 |
baseV[1][i] = 6*vc.a[i]*time+2*vc.b[i]; // second derivative,i.e. acceleration vector |
|
| 405 | 455 |
} |
| 406 | 456 |
|
| 407 | 457 |
computeOrthonormalBase(); |
| ... | ... | |
| 434 | 484 |
{
|
| 435 | 485 |
int non_zeros=0; |
| 436 | 486 |
int last_non_zero=-1; |
| 437 |
float value; |
|
| 438 |
for(int i=0; i<mDimension; i++) |
|
| 439 |
{
|
|
| 440 |
value = baseV[0][i]; |
|
| 487 |
float tmp; |
|
| 441 | 488 |
|
| 442 |
if( value != 0.0f ) |
|
| 489 |
for(int i=0; i<mDimension; i++) |
|
| 490 |
if( baseV[0][i] != 0.0f ) |
|
| 443 | 491 |
{
|
| 444 | 492 |
non_zeros++; |
| 445 | 493 |
last_non_zero=i; |
| 446 | 494 |
} |
| 447 |
} |
|
| 448 |
// velocity is the 0 vector -> two consecutive points we are interpolating |
|
| 449 |
if( non_zeros==0 ) // through are identical -> no noise, set the base to 0 vectors. |
|
| 450 |
{
|
|
| 451 |
for(int i=0; i<mDimension-1; i++) |
|
| 452 |
for(int j=0; j<mDimension; j++) |
|
| 453 |
baseV[i+1][j]= 0.0f; |
|
| 454 |
} |
|
| 495 |
/* |
|
| 496 |
if( last_non_zero != lastNon ) |
|
| 497 |
android.util.Log.e("dynamic", "lastNon="+lastNon+" last_non_zero="+last_non_zero);
|
|
| 498 |
*/ |
|
| 499 |
|
|
| 500 |
if( non_zeros==0 ) /// |
|
| 501 |
{ // velocity is the 0 vector -> two
|
|
| 502 |
for(int i=0; i<mDimension-1; i++) // consecutive points we are interpolating |
|
| 503 |
for(int j=0; j<mDimension; j++) // through are identical -> no noise, |
|
| 504 |
baseV[i+1][j]= 0.0f; // set the base to 0 vectors. |
|
| 505 |
} /// |
|
| 455 | 506 |
else |
| 456 | 507 |
{
|
| 457 |
// We can use (modified!) Gram-Schmidt. |
|
| 458 |
// |
|
| 459 |
// b[0] = b[0] |
|
| 460 |
// b[1] = b[1] - (<b[1],b[0]>/<b[0],b[0]>)*b[0] |
|
| 461 |
// b[2] = b[2] - (<b[2],b[0]>/<b[0],b[0]>)*b[0] - (<b[2],b[1]>/<b[1],b[1]>)*b[1] |
|
| 462 |
// b[3] = b[3] - (<b[3],b[0]>/<b[0],b[0]>)*b[0] - (<b[3],b[1]>/<b[1],b[1]>)*b[1] - (<b[3],b[2]>/<b[2],b[2]>)*b[2] |
|
| 463 |
// (...) |
|
| 464 |
// then b[i] = b[i] / |b[i]| |
|
| 465 |
|
|
| 466 |
float tmp; |
|
| 467 |
|
|
| 468 |
for(int i=1; i<mDimension; i++) /// one iteration computes baseV[i][*], the i-th orthonormal vector. |
|
| 469 |
{
|
|
| 470 |
buf[i-1]=0.0f; |
|
| 471 |
|
|
| 472 |
for(int k=0; k<mDimension; k++) |
|
| 473 |
{
|
|
| 474 |
old[k] = baseV[i][k]; |
|
| 475 |
|
|
| 476 |
if( (i<=last_non_zero && k==i-1) || (i>=(last_non_zero+1) && k==i) ) |
|
| 477 |
baseV[i][k]= baseV[0][last_non_zero]; |
|
| 478 |
else |
|
| 479 |
baseV[i][k]= 0.0f; |
|
| 480 |
|
|
| 481 |
value = baseV[i-1][k]; |
|
| 482 |
buf[i-1] += value*value; |
|
| 483 |
} |
|
| 484 |
|
|
| 485 |
for(int j=0; j<i; j++) |
|
| 486 |
{
|
|
| 487 |
tmp = 0.0f; |
|
| 488 |
|
|
| 489 |
for(int k=0;k<mDimension; k++) |
|
| 490 |
{
|
|
| 491 |
tmp += baseV[i][k]*baseV[j][k]; |
|
| 492 |
} |
|
| 493 |
|
|
| 494 |
tmp /= buf[j]; |
|
| 495 |
|
|
| 496 |
for(int k=0;k<mDimension; k++) |
|
| 497 |
{
|
|
| 498 |
baseV[i][k] -= tmp*baseV[j][k]; |
|
| 499 |
} |
|
| 500 |
} |
|
| 501 |
|
|
| 502 |
if( i>=2 ) checkAngle(i); |
|
| 503 |
} /// end compute baseV[i][*] |
|
| 504 |
|
|
| 505 |
buf[mDimension-1]=0.0f; /// Normalize |
|
| 506 |
for(int k=0; k<mDimension; k++) // |
|
| 507 |
{ //
|
|
| 508 |
value = baseV[mDimension-1][k]; // |
|
| 509 |
buf[mDimension-1] += value*value; // |
|
| 510 |
} // |
|
| 511 |
// |
|
| 512 |
for(int i=1; i<mDimension; i++) // |
|
| 513 |
{ //
|
|
| 514 |
tmp = (float)Math.sqrt(buf[0]/buf[i]); // |
|
| 515 |
// |
|
| 516 |
for(int k=0;k<mDimension; k++) // |
|
| 517 |
{ //
|
|
| 518 |
baseV[i][k] *= tmp; // |
|
| 519 |
} // |
|
| 520 |
} /// End Normalize |
|
| 508 |
for(int i=1; i<mDimension; i++) /// One iteration computes baseV[i][*] |
|
| 509 |
{ // (aka b[i]), the i-th orthonormal vector.
|
|
| 510 |
buf[i-1]=0.0f; // |
|
| 511 |
// We can use (modified!) Gram-Schmidt. |
|
| 512 |
for(int k=0; k<mDimension; k++) // |
|
| 513 |
{ //
|
|
| 514 |
if( i>=2 ) // b[0] = b[0] |
|
| 515 |
{ // b[1] = b[1] - (<b[1],b[0]>/<b[0],b[0]>)*b[0]
|
|
| 516 |
old[k] = baseV[i][k]; // b[2] = b[2] - (<b[2],b[0]>/<b[0],b[0]>)*b[0] - (<b[2],b[1]>/<b[1],b[1]>)*b[1] |
|
| 517 |
baseV[i][k]= (k==i-(last_non_zero>=i?1:0)) ? 1.0f : 0.0f; // b[3] = b[3] - (<b[3],b[0]>/<b[0],b[0]>)*b[0] - (<b[3],b[1]>/<b[1],b[1]>)*b[1] - (<b[3],b[2]>/<b[2],b[2]>)*b[2] |
|
| 518 |
} // (...) |
|
| 519 |
// then b[i] = b[i] / |b[i]| ( Here really b[i] = b[i] / (|b[0]|/|b[i]|) |
|
| 520 |
tmp = baseV[i-1][k]; // |
|
| 521 |
buf[i-1] += tmp*tmp; // |
|
| 522 |
} // |
|
| 523 |
// |
|
| 524 |
for(int j=0; j<i; j++) // |
|
| 525 |
{ //
|
|
| 526 |
tmp = 0.0f; // |
|
| 527 |
for(int k=0;k<mDimension; k++) tmp += baseV[i][k]*baseV[j][k]; // |
|
| 528 |
tmp /= buf[j]; // |
|
| 529 |
for(int k=0;k<mDimension; k++) baseV[i][k] -= tmp*baseV[j][k]; // |
|
| 530 |
} // |
|
| 531 |
// |
|
| 532 |
checkAngle(i); // |
|
| 533 |
} /// end compute baseV[i][*] |
|
| 534 |
|
|
| 535 |
buf[mDimension-1]=0.0f; /// Normalize |
|
| 536 |
for(int k=0; k<mDimension; k++) // |
|
| 537 |
{ //
|
|
| 538 |
tmp = baseV[mDimension-1][k]; // |
|
| 539 |
buf[mDimension-1] += tmp*tmp; // |
|
| 540 |
} // |
|
| 541 |
// |
|
| 542 |
for(int i=1; i<mDimension; i++) // |
|
| 543 |
{ //
|
|
| 544 |
tmp = (float)Math.sqrt(buf[0]/buf[i]); // |
|
| 545 |
for(int k=0;k<mDimension; k++) baseV[i][k] *= tmp; // |
|
| 546 |
} /// End Normalize |
|
| 521 | 547 |
} |
| 522 | 548 |
|
| 549 |
//lastNon = last_non_zero; |
|
| 550 |
|
|
| 523 | 551 |
//printBase("end");
|
| 524 | 552 |
//checkBase(); |
| 525 | 553 |
} |
Also available in: Unified diff
Next fixes for issues with 'jumping' path when noise is on. (and a whole lot of commented out debugging)