TwistyPuzzleLib

///////////////////////////////////////////////////////////////////////////////////////////////////

// Copyright 2020 Leszek Koltunski                                                               //

//                                                                                               //

// This file is part of Magic Cube.                                                              //

//                                                                                               //

// Magic Cube is proprietary software licensed under an EULA which you should have received      //

// along with the code. If not, check https://distorted.org/magic/License-Magic-Cube.html        //

///////////////////////////////////////////////////////////////////////////////////////////////////

package org.distorted.objectlib.helpers;

import org.distorted.library.effect.EffectName;

import org.distorted.library.effect.MatrixEffectMove;

import org.distorted.library.effect.MatrixEffectQuaternion;

import org.distorted.library.effect.MatrixEffectScale;

import org.distorted.library.effect.VertexEffect;

import org.distorted.library.helpers.QuatHelper;

import org.distorted.library.mesh.MeshBase;

import org.distorted.library.mesh.MeshJoined;

import org.distorted.library.mesh.MeshMultigon;

import org.distorted.library.mesh.MeshPolygon;

import org.distorted.library.type.Static3D;

import org.distorted.library.type.Static4D;

import java.util.ArrayList;

///////////////////////////////////////////////////////////////////////////////////////////////////

public class FactoryCubit

  {

  private static FactoryCubit mThis;

  private static final float MAX_CORE_DIFF = 0.01f;

  private static final float[] mBuffer = new float[3];

  private static final float[] mQuat1  = new float[4];

  private static final float[] mQuat2  = new float[4];

  private static final float[] mQuat3  = new float[4];

  public static final String NAME = EffectName.DEFORM.name();

  private static class StickerCoords

    {

    float[][][] outerVertices;

    float[][][] fullVertices;

    float scale;

    boolean outer;

    }

  private static class FaceTransform

    {

    int face;

    int numFaces;

    int sticker;

    float vx,vy,vz;

    float scale;

    float qx,qy,qz,qw;

    }

  private static final ArrayList<FaceTransform> mNewFaceTransf = new ArrayList<>();

  private static final ArrayList<FaceTransform> mOldFaceTransf = new ArrayList<>();

  private static final ArrayList<StickerCoords> mStickerCoords = new ArrayList<>();

///////////////////////////////////////////////////////////////////////////////////////////////////

  private FactoryCubit()

    {

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

// H - height of the band in the middle

// alpha - angle of the edge  [0,90]

// dist - often in a polygon the distance from edge to center is not 1, but something else.

// This is the distance.

// K - where to begin the second, much more flat part of the band. [0,1]

// N - number of bands. N>=3

//

// theory: two distinct parts to the band:

// 1) (0,B) - steep

// 2) (B,1) - flat

//

// In first part, we have y = g(x) ; in second - y = g(f(x)) where

//

// g(x) = sqrt( R^2 - (x-D)^2 ) - R*cos(alpha)

// f(x) = ((D-B)/(1-B)*x + B*(1-D)/(1-B)

// h(x) = R*(sin(alpha) - sin(x))

// R = H/(1-cos(alpha))

// D = H*sin(alpha)

// B = h(K*alpha)

//

// The N points are taken at:

//

// 1) in the second part, there are K2 = (N-3)/3 such points

// 2) in the first - K1 = (N-3) - K2

// 3) also, the 3 points 0,B,1

//

// so we have the sequence A[i] of N points

//

// 0

// h((i+1)*(1-K)*alpha/(K1+1)) (i=0,1,...,K1-1)

// B

// (1-B)*(i+1)/(K2+1) + B   (i=0,i,...,K2-1)

// 1

///////////////////////////////////////////////////////////////////////////////////////////////////

  private float f(float D, float B, float x)

    {

    return ((D-B)*x + B*(1-D))/(1-B);

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private float g(float R, float D, float x, float cosAlpha)

    {

    float d = x-D;

    return (float)(Math.sqrt(R*R-d*d)-R*cosAlpha);

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private float h(float R, float sinAlpha, float x)

    {

    return R*(sinAlpha-(float)Math.sin(x));

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private boolean areNotColinear(float[][] vertices, int index)

    {

    float x1 = vertices[0][0];

    float y1 = vertices[0][1];

    float z1 = vertices[0][2];

    float x2 = vertices[1][0];

    float y2 = vertices[1][1];

    float z2 = vertices[1][2];

    float x3 = vertices[index][0];

    float y3 = vertices[index][1];

    float z3 = vertices[index][2];

    float v1x = x2-x1;

    float v1y = y2-y1;

    float v1z = z2-z1;

    float v2x = x3-x1;

    float v2y = y3-y1;

    float v2z = z3-z1;

    double A = Math.sqrt( (v1x*v1x+v1y*v1y+v1z*v1z) / (v2x*v2x+v2y*v2y+v2z*v2z) );

    return !(v1x==A*v2x && v1y==A*v2y && v1z==A*v2z);

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private void computeNormalVector(float[][] vertices, int index)

    {

    float x1 = vertices[0][0];

    float y1 = vertices[0][1];

    float z1 = vertices[0][2];

    float x2 = vertices[1][0];

    float y2 = vertices[1][1];

    float z2 = vertices[1][2];

    float x3 = vertices[index][0];

    float y3 = vertices[index][1];

    float z3 = vertices[index][2];

    float v1x = x2-x1;

    float v1y = y2-y1;

    float v1z = z2-z1;

    float v2x = x3-x1;

    float v2y = y3-y1;

    float v2z = z3-z1;

    mBuffer[0] = v1y*v2z - v2y*v1z;

    mBuffer[1] = v1z*v2x - v2z*v1x;

    mBuffer[2] = v1x*v2y - v2x*v1y;

    double len = mBuffer[0]*mBuffer[0] + mBuffer[1]*mBuffer[1] + mBuffer[2]*mBuffer[2];

    len = Math.sqrt(len);

    mBuffer[0] /= len;

    mBuffer[1] /= len;

    mBuffer[2] /= len;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private float[][][] computeOuterEdge(float[][][] vertices)

    {

    int[][] edgesUp = MeshMultigon.computeEdgesUp(vertices);

    return MeshMultigon.computeOuterAndHoleVertices(vertices,edgesUp);

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

// polygon

  private void fitInSquare(FaceTransform info, float[][] vert3D, boolean isOuter)

    {

    float minX = Float.MAX_VALUE;

    float maxX =-Float.MAX_VALUE;

    float minY = Float.MAX_VALUE;

    float maxY =-Float.MAX_VALUE;

    for (float[] vert : vert3D)

      {

      float x = vert[0];

      float y = vert[1];

      if (x > maxX) maxX = x;

      if (x < minX) minX = x;

      if (y > maxY) maxY = y;

      if (y < minY) minY = y;

      }

    minX = minX<0 ? -minX:minX;

    maxX = maxX<0 ? -maxX:maxX;

    minY = minY<0 ? -minY:minY;

    maxY = maxY<0 ? -maxY:maxY;

    float max1 = Math.max(minX,minY);

    float max2 = Math.max(maxX,maxY);

    float max3 = Math.max(max1,max2);

    info.scale = max3/0.5f;

    int len = vert3D.length;

    StickerCoords sInfo = new StickerCoords();

    sInfo.outer = isOuter;

    sInfo.scale = info.scale;

    sInfo.outerVertices = new float[1][len][2];

    sInfo.fullVertices = null;

    float[][] t = sInfo.outerVertices[0];

    for( int vertex=0; vertex<len; vertex++ )

      {

      t[vertex][0] = vert3D[vertex][0] / info.scale;

      t[vertex][1] = vert3D[vertex][1] / info.scale;

      }

    mStickerCoords.add(sInfo);

    info.sticker = mStickerCoords.size() -1;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

// multigon

  private void fitInSquare(FaceTransform info, float[][][] vert3D, boolean isOuter)

    {

    float minX = Float.MAX_VALUE;

    float maxX =-Float.MAX_VALUE;

    float minY = Float.MAX_VALUE;

    float maxY =-Float.MAX_VALUE;

    for( float[][] vert : vert3D)

      for( float[] v : vert)

        {

        float x = v[0];

        float y = v[1];

        if (x > maxX) maxX = x;

        if (x < minX) minX = x;

        if (y > maxY) maxY = y;

        if (y < minY) minY = y;

        }

    minX = minX<0 ? -minX:minX;

    maxX = maxX<0 ? -maxX:maxX;

    minY = minY<0 ? -minY:minY;

    maxY = maxY<0 ? -maxY:maxY;

    float max1 = Math.max(minX,minY);

    float max2 = Math.max(maxX,maxY);

    float max3 = Math.max(max1,max2);

    info.scale = max3/0.5f;

    int len = vert3D.length;

    StickerCoords sInfo = new StickerCoords();

    sInfo.outer = isOuter;

    sInfo.scale = info.scale;

    sInfo.fullVertices = new float[len][][];

    for( int comp=0; comp<len; comp++ )

      {

      float[][] vert = vert3D[comp];

      int num = vert.length;

      sInfo.fullVertices[comp] = new float[num][2];

      float[][] t = sInfo.fullVertices[comp];

      for( int vertex=0; vertex<num; vertex++)

        {

        t[vertex][0] = vert[vertex][0] / info.scale;

        t[vertex][1] = vert[vertex][1] / info.scale;

        }

      }

    sInfo.outerVertices = computeOuterEdge(sInfo.fullVertices);

    mStickerCoords.add(sInfo);

    info.sticker = mStickerCoords.size() -1;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

// polygon

  private FaceTransform constructNewTransform(final float[][] vert3D, boolean isOuter, int face, int numFaces)

    {

    FaceTransform ft = new FaceTransform();

    ft.face = face;

    ft.numFaces = numFaces;

    // compute center of gravity

    ft.vx = 0.0f;

    ft.vy = 0.0f;

    ft.vz = 0.0f;

    int len = vert3D.length;

    for (float[] vert : vert3D)

      {

      ft.vx += vert[0];

      ft.vy += vert[1];

      ft.vz += vert[2];

      }

    ft.vx /= len;

    ft.vy /= len;

    ft.vz /= len;

    // move all vertices so that their center of gravity is at (0,0,0)

    for (int i=0; i<len; i++)

      {

      vert3D[i][0] -= ft.vx;

      vert3D[i][1] -= ft.vy;

      vert3D[i][2] -= ft.vz;

      }

    // find 3 non-colinear vertices

    int foundIndex = -1;

    for(int vertex=2; vertex<len; vertex++)

      {

      if( areNotColinear(vert3D,vertex) )

        {

        foundIndex = vertex;

        break;

        }

      }

    // compute the normal vector

    if( foundIndex==-1 )

      {

      StringBuilder sb = new StringBuilder();

      for (float[] floats : vert3D)

        {

        sb.append(' ');

        sb.append("(");

        sb.append(floats[0]);

        sb.append(" ");

        sb.append(floats[1]);

        sb.append(" ");

        sb.append(floats[2]);

        sb.append(")");

        }

      android.util.Log.e("D", "verts: "+sb);

      throw new RuntimeException("all vertices colinear");

      }

    computeNormalVector(vert3D,foundIndex);

    // rotate so that the normal vector becomes (0,0,1)

    float axisX, axisY, axisZ;

    if( mBuffer[0]!=0.0f || mBuffer[1]!=0.0f )

      {

      axisX = -mBuffer[1];

      axisY =  mBuffer[0];

      axisZ = 0.0f;

      float axiLen = axisX*axisX + axisY*axisY;

      axiLen = (float)Math.sqrt(axiLen);

      axisX /= axiLen;

      axisY /= axiLen;

      axisZ /= axiLen;

      }

    else

      {

      axisX = 0.0f;

      axisY = 1.0f;

      axisZ = 0.0f;

      }

    float cosTheta = mBuffer[2];

    float sinTheta = (float)Math.sqrt(1-cosTheta*cosTheta);

    float sinHalfTheta = computeSinHalf(cosTheta);

    float cosHalfTheta = computeCosHalf(sinTheta,cosTheta);

    mQuat1[0] = axisX*sinHalfTheta;

    mQuat1[1] = axisY*sinHalfTheta;

    mQuat1[2] = axisZ*sinHalfTheta;

    mQuat1[3] = cosHalfTheta;

    mQuat2[0] =-axisX*sinHalfTheta;

    mQuat2[1] =-axisY*sinHalfTheta;

    mQuat2[2] =-axisZ*sinHalfTheta;

    mQuat2[3] = cosHalfTheta;

    for (float[] vert : vert3D)

      {

      QuatHelper.quatMultiply(mQuat3, mQuat1, vert  );

      QuatHelper.quatMultiply(  vert, mQuat3, mQuat2);

      }

    // fit the whole thing in a square and remember the scale & 2D vertices

    fitInSquare(ft, vert3D, isOuter);

    // remember the rotation

    ft.qx =-mQuat1[0];

    ft.qy =-mQuat1[1];

    ft.qz =-mQuat1[2];

    ft.qw = mQuat1[3];

    return ft;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

// multigon

  private FaceTransform constructNewTransform(final float[][][] vert3D, boolean isOuter, int face, int numFaces)

    {

    FaceTransform ft = new FaceTransform();

    ft.face = face;

    ft.numFaces = numFaces;

    // compute center of gravity

    ft.vx = 0.0f;

    ft.vy = 0.0f;

    ft.vz = 0.0f;

    int len = 0;

    for( float[][] vert : vert3D )

      for( float[] v : vert )

        {

        ft.vx += v[0];

        ft.vy += v[1];

        ft.vz += v[2];

        len++;

        }

    ft.vx /= len;

    ft.vy /= len;

    ft.vz /= len;

    // move all vertices so that their center of gravity is at (0,0,0)

    for( float[][] vert : vert3D )

      for( float[] v : vert )

        {

        v[0] -= ft.vx;

        v[1] -= ft.vy;

        v[2] -= ft.vz;

        }

    // find 3 non-colinear vertices

    int foundIndex = -1;

    len = vert3D[0].length;

    for(int vertex=2; vertex<len; vertex++)

      {

      if( areNotColinear(vert3D[0],vertex) )

        {

        foundIndex = vertex;

        break;

        }

      }

    // compute the normal vector

    if( foundIndex==-1 )

      {

      StringBuilder sb = new StringBuilder();

      for (float[] v : vert3D[0])

        {

        sb.append(' ');

        sb.append("(");

        sb.append(v[0]);

        sb.append(" ");

        sb.append(v[1]);

        sb.append(" ");

        sb.append(v[2]);

        sb.append(")");

        }

      android.util.Log.e("D", "verts: "+sb);

      throw new RuntimeException("all vertices colinear");

      }

    computeNormalVector(vert3D[0],foundIndex);

    // rotate so that the normal vector becomes (0,0,1)

    float axisX, axisY, axisZ;

    if( mBuffer[0]!=0.0f || mBuffer[1]!=0.0f )

      {

      axisX = -mBuffer[1];

      axisY =  mBuffer[0];

      axisZ = 0.0f;

      float axiLen = axisX*axisX + axisY*axisY;

      axiLen = (float)Math.sqrt(axiLen);

      axisX /= axiLen;

      axisY /= axiLen;

      axisZ /= axiLen;

      }

    else

      {

      axisX = 0.0f;

      axisY = 1.0f;

      axisZ = 0.0f;

      }

    float cosTheta = mBuffer[2];

    float sinTheta = (float)Math.sqrt(1-cosTheta*cosTheta);

    float sinHalfTheta = computeSinHalf(cosTheta);

    float cosHalfTheta = computeCosHalf(sinTheta,cosTheta);

    mQuat1[0] = axisX*sinHalfTheta;

    mQuat1[1] = axisY*sinHalfTheta;

    mQuat1[2] = axisZ*sinHalfTheta;

    mQuat1[3] = cosHalfTheta;

    mQuat2[0] =-axisX*sinHalfTheta;

    mQuat2[1] =-axisY*sinHalfTheta;

    mQuat2[2] =-axisZ*sinHalfTheta;

    mQuat2[3] = cosHalfTheta;

    for( float[][] vert : vert3D)

      for( float[] v : vert)

        {

        QuatHelper.quatMultiply(mQuat3, mQuat1, v  );

        QuatHelper.quatMultiply(  v, mQuat3, mQuat2);

        }

    // fit the whole thing in a square and remember the scale & 2D vertices

    fitInSquare(ft, vert3D, isOuter);

    // remember the rotation

    ft.qx =-mQuat1[0];

    ft.qy =-mQuat1[1];

    ft.qz =-mQuat1[2];

    ft.qw = mQuat1[3];

    return ft;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private void rotateAllVertices(float[][] result, int len, float[][] vertices, float sin, float cos)

    {

    for(int i=0; i<len; i++)

      {

      float x = vertices[i][0];

      float y = vertices[i][1];

      result[i][0] = x*cos - y*sin;

      result[i][1] = x*sin + y*cos;

      }

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private float computeScale(float[][] v1, float[][] v2, int v1i, int v2i)

    {

    float v1x = v1[v1i][0];

    float v1y = v1[v1i][1];

    float v2x = v2[v2i][0];

    float v2y = v2[v2i][1];

    float lenSq1 = v1x*v1x + v1y*v1y;

    float lenSq2 = v2x*v2x + v2y*v2y;

    return (float)Math.sqrt(lenSq2/lenSq1);

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

// valid for 0<angle<2*PI

  private float computeSinHalf(float cos)

    {

    return (float)Math.sqrt((1-cos)/2);

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

// valid for 0<angle<2*PI

  private float computeCosHalf(float sin, float cos)

    {

    float cosHalf = (float)Math.sqrt((1+cos)/2);

    return sin<0 ? -cosHalf : cosHalf;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private int computeRotatedIndex(int oldVertex, int len, int rotatedVertex)

    {

    int v = (rotatedVertex + oldVertex);

    if( v>=len ) v-=len;

    if( v< 0   ) v+=len;

    return v;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private boolean isScaledVersionOf(float[][] newVert, float[][] oldVert, int len, int vertex)

    {

    int newZeroIndex = computeRotatedIndex(0,len,vertex);

    float EPSILON = 0.001f;

    float scale = computeScale(newVert,oldVert,newZeroIndex,0);

    for(int i=1; i<len; i++)

      {

      int index = computeRotatedIndex(i,len,vertex);

      float horz = oldVert[i][0] - scale*newVert[index][0];

      float vert = oldVert[i][1] - scale*newVert[index][1];

      if( horz>EPSILON || horz<-EPSILON || vert>EPSILON || vert<-EPSILON ) return false;

      }

    return true;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private void correctInfo(FaceTransform info, float scale, float sin, float cos, int oldSticker)

    {

    mStickerCoords.remove(info.sticker);

    info.sticker = oldSticker;

    info.scale  *= scale;

    mQuat1[0] = info.qx;

    mQuat1[1] = info.qy;

    mQuat1[2] = info.qz;

    mQuat1[3] = info.qw;

    float sinHalf = computeSinHalf(cos);

    float cosHalf = computeCosHalf(sin,cos);

    mQuat2[0] = 0.0f;

    mQuat2[1] = 0.0f;

    mQuat2[2] = sinHalf;

    mQuat2[3] = cosHalf;

    QuatHelper.quatMultiply( mQuat3, mQuat1, mQuat2 );

    info.qx = mQuat3[0];

    info.qy = mQuat3[1];

    info.qz = mQuat3[2];

    info.qw = mQuat3[3];

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private boolean foundVertex(FaceTransform info, float[][] buffer, float[][] newVert, float[][] oldVert, int oldSticker)

    {

    float lenOld=0.0f, oldX=0.0f, oldY=0.0f;

    int len = newVert.length;

    for(float[] ov : oldVert)

      {

      oldX = ov[0];

      oldY = ov[1];

      lenOld = (float) Math.sqrt(oldX*oldX+oldY*oldY);

      if(lenOld!=0) break;

      }

    for(int vertex=0; vertex<len; vertex++)

      {

      float newX = newVert[vertex][0];

      float newY = newVert[vertex][1];

      float lenNew = (float)Math.sqrt(newX*newX + newY*newY);

      if( lenNew!=0 )

        {

        float cos = (float)QuatHelper.computeCos( oldX, oldY, newX, newY, lenNew, lenOld);

        float sin = (float)QuatHelper.computeSin( oldX, oldY, newX, newY, lenNew, lenOld);

        rotateAllVertices(buffer,len,newVert,sin,cos);

        if( isScaledVersionOf(buffer,oldVert,len,vertex) )

          {

          int newZeroIndex = computeRotatedIndex(0,len,vertex);

          float scale = computeScale(oldVert,newVert,0,newZeroIndex);

          correctInfo(info,scale,sin,cos,oldSticker);

          return true;

          }

        }

      }

    return false;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

  private float computeCoreDistance(float[][] verts)

    {

    float ret = 0.0f;

    float centerX=0.0f,centerY=0.0f;

    int len = verts.length;

    for(float[] vert : verts)

      {

      centerX += vert[0];

      centerY += vert[1];

      }

    centerX /= len;

    centerY /= len;

    for(float[] vert : verts)

      {

      float distX = centerX-vert[0];

      float distY = centerY-vert[1];

      ret += (float) Math.sqrt(distX*distX+distY*distY);

      }

    return ret;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

// even if this is a multigon, then StickerCoords.outerVertices is the outer edge!

// (see fitInSquare multigon variant)

  private boolean successfullyCollapsedStickers(final FaceTransform newInfo, final FaceTransform oldInfo)

    {

    StickerCoords sNewInfo = mStickerCoords.get(newInfo.sticker);

    StickerCoords sOldInfo = mStickerCoords.get(oldInfo.sticker);

    if( sNewInfo.outerVertices.length>1 || sOldInfo.outerVertices.length>1 ) // the 'outer' path is composed of more than

      {                                                                      // one segment, i.e. the sticker has holes.

      return false;                                                          // do not collapse such stickers.

      }

    float[][] newVert = sNewInfo.outerVertices[0];

    float[][] oldVert = sOldInfo.outerVertices[0];

    int oldLen = oldVert.length;

    int newLen = newVert.length;

    if( oldLen==newLen )

      {

      float coreDistOld = computeCoreDistance(oldVert);                     // the two stickers are at different scales,

      float coreDistNew = computeCoreDistance(newVert);                     // so even if they are in fact the same, do not

      float diff = (coreDistOld*oldInfo.scale)/(coreDistNew*newInfo.scale); // collapse them into one. Example: Master Skewb

      if( diff<1.0-MAX_CORE_DIFF || diff>1.0+MAX_CORE_DIFF ) return false;  // and two triangular stickers of different size.

      int oldSticker = oldInfo.sticker;

      float[][] buffer = new float[oldLen][2];

      if( foundVertex(newInfo, buffer, newVert, oldVert, oldSticker) )

        {

        if( sNewInfo.outer ) sOldInfo.outer = true;

        return true;

        }

      }

    return false;

    }

///////////////////////////////////////////////////////////////////////////////////////////////////

// polygon

  private float[][] constructVert(float[][] vertices, int[] index)

    {

    int len = index.length;

    float[][] ret = new float[len][4];

    for(int i=0; i<len; i++)

      {

      float[] tmp = vertices[index[i]];

      ret[i][0] = tmp[0];

      ret[i][1] = tmp[1];

      ret[i][2] = tmp[2];