commit 6a2ebb18d902cd039f59fe3080feef748272c02b
Author: Leszek Koltunski <leszek@distorted.org>
Date:   Wed Nov 2 13:24:45 2016 +0000

    Next fixes for issues with 'jumping' path when noise is on. (and a whole lot of commented out debugging)

diff --git a/src/main/java/org/distorted/library/type/Dynamic.java b/src/main/java/org/distorted/library/type/Dynamic.java
index 85c06b8..ee1cf48 100644
--- a/src/main/java/org/distorted/library/type/Dynamic.java
+++ b/src/main/java/org/distorted/library/type/Dynamic.java
@@ -46,7 +46,7 @@ import java.util.Vector;
 // we have the solution:  X(t) = at^3 + bt^2 + ct + d where
 // a =  2*P[i](x) +   w[i](x) - 2*P[i+1](x) + w[i+1](x)
 // b = -3*P[i](x) - 2*w[i](x) + 3*P[i+1](x) - w[i+1](x)
-// c = w[i](x)<br>
+// c = w[i](x)
 // d = P[i](x)
 //
 // and similarly Y(t) and Z(t).
@@ -66,11 +66,6 @@ public abstract class Dynamic
    */
   public static final int MODE_JUMP = 2; 
 
-  protected static Random mRnd = new Random();
-  
-  protected static final int NUM_NOISE = 5; // used iff mNoise>0.0. Number of intermediary points between each pair of adjacent vectors
-                                            // where we randomize noise factors to make the way between the two vectors not so smooth.
-
   protected int mDimension;
   protected int numPoints;
   protected int mVecCurr;    
@@ -103,8 +98,6 @@ public abstract class Dynamic
   protected float[] mFactor;
   protected float[] mNoise;
   protected float[][] baseV;
-  private float[] buf;
-  private float[] old;
 
   ///////////////////////////////////////////////////////////////////////////////////////////////////
   // 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,6 +128,14 @@ public abstract class Dynamic
   protected Vector<VectorCache> vc;
   protected VectorCache tmp1, tmp2;
 
+  private float[] buf;
+  private float[] old;
+  private static Random mRnd = new Random();
+  private static final int NUM_NOISE = 5; // used iff mNoise>0.0. Number of intermediary points between each pair of adjacent vectors
+                                          // where we randomize noise factors to make the way between the two vectors not so smooth.
+
+//private int lastNon;
+
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 // hide this from Javadoc
   
@@ -270,7 +271,25 @@ public abstract class Dynamic
 
   private void checkBase()
     {
-    float tmp;
+    float tmp, cosA;
+    float[] len= new float[mDimension];
+    boolean error=false;
+
+    for(int i=0; i<mDimension; i++)
+      {
+      len[i] = 0.0f;
+
+      for(int k=0; k<mDimension; k++)
+        {
+        len[i] += baseV[i][k]*baseV[i][k];
+        }
+
+      if( len[i] == 0.0f || len[0]/len[i] < 0.95f || len[0]/len[i]>1.05f )
+        {
+        android.util.Log.e("dynamic", "length of vector "+i+" : "+Math.sqrt(len[i]));
+        error = true;
+        }
+      }
 
     for(int i=0; i<mDimension; i++)
       for(int j=i+1; j<mDimension; j++)
@@ -282,20 +301,16 @@ public abstract class Dynamic
           tmp += baseV[i][k]*baseV[j][k];
           }
 
-        android.util.Log.e("dynamic", "vectors "+i+" and "+j+" : "+tmp);
-        }
-
-    for(int i=0; i<mDimension; i++)
-      {
-      tmp = 0.0f;
+        cosA = ( (len[i]==0.0f || len[j]==0.0f) ? 0.0f : tmp/(float)Math.sqrt(len[i]*len[j]));
 
-      for(int k=0; k<mDimension; k++)
-        {
-        tmp += baseV[i][k]*baseV[i][k];
+        if( cosA > 0.05f || cosA < -0.05f )
+          {
+          android.util.Log.e("dynamic", "cos angle between vectors "+i+" and "+j+" : "+cosA);
+          error = true;
+          }
         }
 
-      android.util.Log.e("dynamic", "length of vector "+i+" : "+Math.sqrt(tmp));
-      }
+    if( error ) printBase("");
     }
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////
@@ -393,6 +408,40 @@ public abstract class Dynamic
     computeOrthonormalBase();
     }
 
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// debugging
+/*
+  protected void computeOrthonormalBaseMoreDebug(float time,VectorCache vc)
+    {
+    for(int i=0; i<mDimension; i++)
+      {
+      baseV[0][i] = (3*vc.a[i]*time+2*vc.b[i])*time+vc.c[i];   // first derivative, i.e. velocity vector
+      baseV[1][i] =  6*vc.a[i]*time+2*vc.b[i];                 // second derivative,i.e. acceleration vector
+      }
+
+    float av=0.0f, vv=0.0f;
+
+    android.util.Log.e("dyn3D", " ==>  velocity     ("+baseV[0][0]+","+baseV[0][1]+","+baseV[0][2]+")");
+    android.util.Log.e("dyn3D", " ==>  acceleration ("+baseV[1][0]+","+baseV[1][1]+","+baseV[1][2]+")");
+
+    for(int k=0; k<mDimension; k++)
+      {
+      vv += baseV[0][k]*baseV[0][k];
+      av += baseV[1][k]*baseV[0][k];
+      }
+
+    android.util.Log.e("dyn3D", " ==>  av: "+av+" vv="+vv);
+
+    av /= vv;
+
+    for(int k=0;k<mDimension; k++)
+      {
+      baseV[1][k] -= av*baseV[0][k];
+      }
+
+    android.util.Log.e("dyn3D", " ==>  second base ("+baseV[1][0]+","+baseV[1][1]+","+baseV[1][2]+")");
+    }
+*/
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 // helper function in case we are interpolating through more than 2 points
 
@@ -400,8 +449,9 @@ public abstract class Dynamic
     {
     for(int i=0; i<mDimension; i++)
       {
-      baseV[0][i] = (3*vc.a[i]*time+2*vc.b[i])*time+vc.c[i];
-      baseV[1][i] =  6*vc.a[i]*time+2*vc.b[i];
+      baseV[0][i] = (3*vc.a[i]*time+2*vc.b[i])*time+vc.c[i];   // first derivative, i.e. velocity vector
+      old[i]      = baseV[1][i];
+      baseV[1][i] =  6*vc.a[i]*time+2*vc.b[i];                 // second derivative,i.e. acceleration vector
       }
 
     computeOrthonormalBase();
@@ -434,92 +484,70 @@ public abstract class Dynamic
     {
     int non_zeros=0;
     int last_non_zero=-1;
-    float value;
-    for(int i=0; i<mDimension; i++)
-      {
-      value = baseV[0][i];
+    float tmp;
 
-      if( value != 0.0f )
+    for(int i=0; i<mDimension; i++)
+      if( baseV[0][i] != 0.0f )
         {
         non_zeros++;
         last_non_zero=i;
         }
-      }
-                         // velocity is the 0 vector -> two consecutive points we are interpolating
-    if( non_zeros==0 )   // through are identical -> no noise, set the base to 0 vectors.
-      {
-      for(int i=0; i<mDimension-1; i++)
-        for(int j=0; j<mDimension; j++)
-          baseV[i+1][j]= 0.0f;
-      }
+/*
+if( last_non_zero != lastNon )
+  android.util.Log.e("dynamic", "lastNon="+lastNon+" last_non_zero="+last_non_zero);
+*/
+
+    if( non_zeros==0 )                                                    ///
+      {                                                                   //  velocity is the 0 vector -> two
+      for(int i=0; i<mDimension-1; i++)                                   //  consecutive points we are interpolating
+        for(int j=0; j<mDimension; j++)                                   //  through are identical -> no noise,
+          baseV[i+1][j]= 0.0f;                                            //  set the base to 0 vectors.
+      }                                                                   ///
     else
       {
-      // We can use (modified!) Gram-Schmidt.
-      //
-      // b[0] = b[0]
-      // b[1] = b[1] - (<b[1],b[0]>/<b[0],b[0]>)*b[0]
-      // b[2] = b[2] - (<b[2],b[0]>/<b[0],b[0]>)*b[0] - (<b[2],b[1]>/<b[1],b[1]>)*b[1]
-      // 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]
-      // (...)
-      // then b[i] = b[i] / |b[i]|
-
-      float tmp;
-
-      for(int i=1; i<mDimension; i++)          /// one iteration computes baseV[i][*], the i-th orthonormal vector.
-        {
-        buf[i-1]=0.0f;
-
-        for(int k=0; k<mDimension; k++)
-          {
-          old[k] = baseV[i][k];
-
-          if( (i<=last_non_zero && k==i-1) || (i>=(last_non_zero+1) && k==i) )
-            baseV[i][k]= baseV[0][last_non_zero];
-          else
-            baseV[i][k]= 0.0f;
-
-          value = baseV[i-1][k];
-          buf[i-1] += value*value;
-          }
-
-        for(int j=0; j<i; j++)
-          {
-          tmp = 0.0f;
-
-          for(int k=0;k<mDimension; k++)
-            {
-            tmp += baseV[i][k]*baseV[j][k];
-            }
-
-          tmp /= buf[j];
-
-          for(int k=0;k<mDimension; k++)
-            {
-            baseV[i][k] -= tmp*baseV[j][k];
-            }
-          }
-
-        if( i>=2 ) checkAngle(i);
-        }                                       /// end compute baseV[i][*]
-
-      buf[mDimension-1]=0.0f;                   /// Normalize
-      for(int k=0; k<mDimension; k++)           //
-        {                                       //
-        value = baseV[mDimension-1][k];         //
-        buf[mDimension-1] += value*value;       //
-        }                                       //
-                                                //
-      for(int i=1; i<mDimension; i++)           //
-        {                                       //
-        tmp = (float)Math.sqrt(buf[0]/buf[i]);  //
-                                                //
-        for(int k=0;k<mDimension; k++)          //
-          {                                     //
-          baseV[i][k] *= tmp;                   //
-          }                                     //
-        }                                       /// End Normalize
+      for(int i=1; i<mDimension; i++)                                     /// One iteration computes baseV[i][*]
+        {                                                                 //  (aka b[i]), the i-th orthonormal vector.
+        buf[i-1]=0.0f;                                                    //
+                                                                          //  We can use (modified!) Gram-Schmidt.
+        for(int k=0; k<mDimension; k++)                                   //
+          {                                                               //
+          if( i>=2 )                                                      //  b[0] = b[0]
+            {                                                             //  b[1] = b[1] - (<b[1],b[0]>/<b[0],b[0]>)*b[0]
+            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]
+            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]
+            }                                                             //  (...)
+                                                                          //  then b[i] = b[i] / |b[i]|  ( Here really b[i] = b[i] / (|b[0]|/|b[i]|)
+          tmp = baseV[i-1][k];                                            //
+          buf[i-1] += tmp*tmp;                                            //
+          }                                                               //
+                                                                          //
+        for(int j=0; j<i; j++)                                            //
+          {                                                               //
+          tmp = 0.0f;                                                     //
+          for(int k=0;k<mDimension; k++) tmp += baseV[i][k]*baseV[j][k];  //
+          tmp /= buf[j];                                                  //
+          for(int k=0;k<mDimension; k++) baseV[i][k] -= tmp*baseV[j][k];  //
+          }                                                               //
+                                                                          //
+        checkAngle(i);                                                    //
+        }                                                                 /// end compute baseV[i][*]
+
+      buf[mDimension-1]=0.0f;                                             /// Normalize
+      for(int k=0; k<mDimension; k++)                                     //
+        {                                                                 //
+        tmp = baseV[mDimension-1][k];                                     //
+        buf[mDimension-1] += tmp*tmp;                                     //
+        }                                                                 //
+                                                                          //
+      for(int i=1; i<mDimension; i++)                                     //
+        {                                                                 //
+        tmp = (float)Math.sqrt(buf[0]/buf[i]);                            //
+        for(int k=0;k<mDimension; k++) baseV[i][k] *= tmp;                //
+        }                                                                 /// End Normalize
       }
 
+//lastNon = last_non_zero;
+
     //printBase("end");
     //checkBase();
     }
diff --git a/src/main/java/org/distorted/library/type/Dynamic3D.java b/src/main/java/org/distorted/library/type/Dynamic3D.java
index 9429684..747018d 100644
--- a/src/main/java/org/distorted/library/type/Dynamic3D.java
+++ b/src/main/java/org/distorted/library/type/Dynamic3D.java
@@ -31,7 +31,14 @@ public class Dynamic3D extends Dynamic implements Data3D
   {
   private Vector<Static3D> vv;
   private Static3D prev, curr, next;
-
+/*
+private float a0,a1,a2;
+private float f0, f1;
+private float b00,b01,b02;
+private float b10,b11,b12;
+private float b20,b21,b22;
+private float oldTime;
+*/
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 // no array bounds checking!
   
@@ -516,6 +523,64 @@ public class Dynamic3D extends Dynamic implements Data3D
                   buffer[offset  ]= ((tmp1.a[0]*time+tmp1.b[0])*time+tmp1.c[0])*time+tmp1.d[0] + (baseV[1][0]*mFactor[0] + baseV[2][0]*mFactor[1]);
                   buffer[offset+1]= ((tmp1.a[1]*time+tmp1.b[1])*time+tmp1.c[1])*time+tmp1.d[1] + (baseV[1][1]*mFactor[0] + baseV[2][1]*mFactor[1]);
                   buffer[offset+2]= ((tmp1.a[2]*time+tmp1.b[2])*time+tmp1.c[2])*time+tmp1.d[2] + (baseV[1][2]*mFactor[0] + baseV[2][2]*mFactor[1]);
+/*
+                  float d0 = buffer[offset+0] - a0;
+                  float d1 = buffer[offset+1] - a1;
+                  float d2 = buffer[offset+2] - a2;
+
+                  float distSQ = d0*d0+d1*d1+d2*d2;
+
+                  if( distSQ>500.0f )
+                    {
+                    android.util.Log.e("dyn3D", "distSQ="+distSQ);
+                    android.util.Log.e("dyn3D", "factors old0="+f0+" new0="+mFactor[0]+" old1="+f1+" new1="+mFactor[1]);
+                    android.util.Log.e("dyn3D", "first  base: old ("+b00+","+b01+","+b02+") new ("+baseV[0][0]+","+baseV[0][1]+","+baseV[0][2]+")");
+                    android.util.Log.e("dyn3D", "second base: old ("+b10+","+b11+","+b12+") new ("+baseV[1][0]+","+baseV[1][1]+","+baseV[1][2]+")");
+                    android.util.Log.e("dyn3D", "third  base: old ("+b20+","+b21+","+b22+") new ("+baseV[2][0]+","+baseV[2][1]+","+baseV[2][2]+")");
+
+                    String s1= "velocity: old (";
+                    String s2= "acceleration: old (";
+
+                    for(int i=0; i<mDimension; i++)
+                      {
+                      s1 += (((3*tmp1.a[i]*oldTime+2*tmp1.b[i])*oldTime+tmp1.c[i])+(i==mDimension-1 ? ") new (":","));
+                      s2 += ( (6*tmp1.a[i]*oldTime+2*tmp1.b[i])                   +(i==mDimension-1 ? ") new (":","));
+                      }
+
+                    for(int i=0; i<mDimension; i++)
+                      {
+                      s1 += (((3*tmp1.a[i]*time+2*tmp1.b[i])*time+tmp1.c[i])+(i==mDimension-1 ? ")":","));
+                      s2 += ( (6*tmp1.a[i]*time+2*tmp1.b[i])                +(i==mDimension-1 ? ")":","));
+                      }
+
+                    android.util.Log.e("dyn3D", s1);
+                    android.util.Log.e("dyn3D", s2);
+
+                    computeOrthonormalBaseMoreDebug(oldTime,tmp1);
+                    computeOrthonormalBaseMoreDebug(   time,tmp1);
+                    }
+
+                  a0 = buffer[offset+0];
+                  a1 = buffer[offset+1];
+                  a2 = buffer[offset+2];
+
+                  f0 = mFactor[0];
+                  f1 = mFactor[1];
+
+                  b00 = baseV[0][0];
+                  b01 = baseV[0][1];
+                  b02 = baseV[0][2];
+
+                  b10 = baseV[1][0];
+                  b11 = baseV[1][1];
+                  b12 = baseV[1][2];
+
+                  b20 = baseV[2][0];
+                  b21 = baseV[2][1];
+                  b22 = baseV[2][2];
+
+                  oldTime = time;
+*/
                   }
                 else
                   {