TwistyPuzzleLib

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

// Copyright 2023 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.type.Static3D;

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

public class FactoryShape

  {

  private static final float MAXERROR = 0.0001f;

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

// return arithmetic average of the vertices.

  private static float[] computeCenterOfMass(float[][] vertices, int num, int[] indices)

    {

    float[] ret = new float[3];

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

      {

      int ind = indices[i];

      float[] v = vertices[ind];

      ret[0] += v[0];

      ret[1] += v[1];

      ret[2] += v[2];

      }

    ret[0] /= num;

    ret[1] /= num;

    ret[2] /= num;

    return ret;

    }

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

// Return true iff 'vertex' lies in the plane 'plane'

  private static boolean vertexOnPlane(float[] vertex, float[] plane)

    {

    float d = vertex[0]*plane[0] + vertex[1]*plane[1] + vertex[2]*plane[2] - plane[3];

    return (d>=-MAXERROR && d<=MAXERROR);

    }

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

// return true iff points p0 and p1 are on different sides of plane 'plane'. [ i.e. every way from

// p0 to p1, always has to pass through at least one point from 'plane' ]

  private static boolean isOnDifferentSides(float[] p0, float[] p1, float[] plane)

    {

    float d0 = p0[0]*plane[0] + p0[1]*plane[1] + p0[2]*plane[2] - plane[3];

    float d1 = p1[0]*plane[0] + p1[1]*plane[1] + p1[2]*plane[2] - plane[3];

    if( d0>=-MAXERROR && d0<=MAXERROR ) return false;

    if( d0>MAXERROR ) return d1<-MAXERROR;

    return d1>MAXERROR;

    }

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

// return indices 'indices', but cut to only 'num' of them [original 'indices' array can be longer]

// and arranged so that the 'vertices' those indices are indices for are in counter-clockwise order

// [starting from an arbitrary one].

//

// a) for every vertex, compute the angle the vector from 'center' to this vertex forms with an

//    arbitrary line [for example the vector from center to the first vertex]

// b) sort the vertices by this angle [ascending]

  private static int[] arrangeCCW(float[] plane, float[] internal, float[][] vertices, int num, int[] indices)

    {

    float[] angles = new float[num];

    float[] center = computeCenterOfMass(vertices,num,indices);

    int j= indices[0];

    float vx = vertices[j][0] - center[0];

    float vy = vertices[j][1] - center[1];

    float vz = vertices[j][2] - center[2];

    float len = (float)Math.sqrt(vx*vx + vy*vy + vz*vz);

    vx /= len;

    vy /= len;

    vz /= len;

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

      {

      int index = indices[i];

      float[] v = vertices[index];

      float wx = v[0]-center[0];

      float wy = v[1]-center[1];

      float wz = v[2]-center[2];

      float l = (float)Math.sqrt(wx*wx + wy*wy + wz*wz);

      wx /= l;

      wy /= l;

      wz /= l;

      angles[i] = computeAngle(plane,internal,vx,vy,vz,wx,wy,wz);

      }

    int[] ret = new int[num];

    for(int i=0; i<num; i++) ret[i] = indices[i];

    bubbleSort(angles,ret,num);

    return ret;

    }

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

// strange version of bubbleSort where we sort array 'arr2' according to values of different array

// 'arr1'.

  private static void bubbleSort(float[] arr1, int[] arr2, int n)

    {

    int i, j, temp;

    boolean swapped;

    float tmp;

    for( i=0; i<n-1; i++ )

      {

      swapped = false;

      for( j=0; j<n-i-1; j++ )

        {

        if( arr1[j] > arr1[j+1] )

          {

          tmp      = arr1[j];

          arr1[j]  = arr1[j+1];

          arr1[j+1]= tmp;

          temp     = arr2[j];

          arr2[j]  = arr2[j+1];

          arr2[j+1]= temp;

          swapped  = true;

          }

        }

      if( !swapped ) break;

      }

    }

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

// given a plane with normal vector 'normal', and two in-plane vectors p=(px,py,pz) and

// r=(rx,ry,rz) [in-plane, i.e. cross(p,r) is parallel to 'normal' ] compute the angle between p and r.

//

// the angle belongs to <0,2PI) and goes from p to r counter-clockwise (according to the 'normal' -

// i.e. look at the situation so that the normal points at you and track the angle from p to r)

//

// vectors p and r are guaranteed to be normalized.

//

// 1. compute scalar product of p and r

// 2. result is the cosine of the 'smaller' angle between the two

// 3. a = compute arc cos of this

// 4. return a or (2PI-a), depending on if cross(p,r) agrees in direction with the normal, or not.

//    'point' is the 'internalPoint' --> the normal needs to point away from this, which is always

//    the case in case of the external walls, but not in case of the internal cuts.

  private static float computeAngle(float[] plane, float[] point,

                                    float px, float py, float pz, float rx, float ry, float rz)

    {

    float s = px*rx + py*ry + pz*rz;

    if( s> 1 ) s= 1;

    if( s<-1 ) s=-1;

    float a = (float) Math.acos(s);

    float[] cross = crossProduct(px,py,pz,rx,ry,rz);

    float scalar1 = cross[0]*plane[0] + cross[1]*plane[1] + cross[2]*plane[2];

    float scalar2 = point[0]*plane[0] + point[1]*plane[1] + point[2]*plane[2];

    return ((scalar1>=0)^(scalar2>=plane[3])) ? a : (float)(2*Math.PI - a);

    }

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

  private static float[] crossProduct(float a1, float a2, float a3, float b1, float b2, float b3)

    {

    float[] ret = new float[3];

    ret[0] = a2*b3 - a3*b2;

    ret[1] = a3*b1 - a1*b3;

    ret[2] = a1*b2 - a2*b1;

    return ret;

    }

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

// if normals to planes p1,p2,p3 are linearly independent, than those planes intersect in one point.

// Return this point. Otherwise, return null.

  private static float[] computeCommonPoint(float[] p1, float[] p2, float[] p3)

    {

    float det = determinant(p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],p3[0],p3[1],p3[2]);

    if( det>=-MAXERROR && det<=MAXERROR ) return null;

    float deX = determinant(p1[3],p1[1],p1[2],p2[3],p2[1],p2[2],p3[3],p3[1],p3[2]);

    float deY = determinant(p1[0],p1[3],p1[2],p2[0],p2[3],p2[2],p3[0],p3[3],p3[2]);

    float deZ = determinant(p1[0],p1[1],p1[3],p2[0],p2[1],p2[3],p3[0],p3[1],p3[3]);

    return new float[] { deX/det, deY/det, deZ/det };

    }

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

// a1 a2 a3 a1 a2

// b1 b2 b3 b1 b2

// c1 c2 c3 c1 c2

  private static float determinant(float a1, float a2, float a3,

                                   float b1, float b2, float b3,

                                   float c1, float c2, float c3 )

    {

    float det = 0;

    det += a1*b2*c3;

    det += a2*b3*c1;

    det += a3*b1*c2;

    det -= a3*b2*c1;

    det -= a1*b3*c2;

    det -= a2*b1*c3;

    return det;

    }

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

// how many faces does the point (x,y,z) belong to? (i.e. 3-->corner, 2-->edge, 1--> face point,

// 0 --> internal

  private static int numberInFaces(Static3D[] faceAxis, float[] dist3D, float x, float y, float z)

    {

    int numA = faceAxis.length;

    int ret = 0;

    for(int a=0; a<numA; a++)

      {

      Static3D ax = faceAxis[a];

      float dist = x*ax.get0() + y*ax.get1() + z*ax.get2();

      float diff = dist-dist3D[a];

      if( diff>-MAXERROR && diff<MAXERROR ) ret++;

      }

    return ret;

    }

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

// does the (x,y,z) point belong to any of the faces defined by (faceAxis,dist) ?

  private static boolean isInFaces(Static3D[] faceAxis, float[] dist3D, float x, float y, float z)

    {

    int numA = faceAxis.length;

    for(int a=0; a<numA; a++)

      {

      Static3D ax = faceAxis[a];

      float dist = x*ax.get0() + y*ax.get1() + z*ax.get2();

      float diff = dist-dist3D[a];

      if( diff>-MAXERROR && diff<MAXERROR ) return true;

      }

    return false;

    }

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

  private static boolean isDifferent(float[][] table, int num, float[] vert)

    {

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

      {

      float[] t = table[i];

      float dx = t[0]-vert[0];

      float dy = t[1]-vert[1];

      float dz = t[2]-vert[2];

      float dist = dx*dx + dy*dy + dz*dz;

      if( dist>=-MAXERROR && dist<=MAXERROR ) return false;

      }

    return true;

    }

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

// PUBLIC API

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

  public static float[][] computePotentialVertices(float[][] cutPlanes)

    {

    int numPlanes = cutPlanes.length;

    int numRet = numPlanes*(numPlanes-1)*(numPlanes-2)/6;

    float[][] ret = new float[numRet][];

    int index = 0;

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

      for(int j=i+1; j<numPlanes; j++)

        for(int k=j+1; k<numPlanes; k++)

          {

          float[] vert = computeCommonPoint(cutPlanes[i],cutPlanes[j],cutPlanes[k]);

          if( vert!=null && isDifferent(ret,index,vert) )

            {

            ret[index] = vert;

            index++;

            }

          }

    if( index<numRet )

      {

      float[][] ret2 = new float[index][];

      for(int i=0; i<index; i++) ret2[i] = ret[i];

      return ret2;

      }

    return ret;

    }

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

// return a table of indices.length ints.

// Nth int is 0 if the face defined by indices[N] is an external one; 1 otherwise.

  public static int[] produceBandIndices(float[][] vertices, float[] position, int[][] indices, Static3D[] faceAxis, float[] dist3D )

    {

    int numF = indices.length;

    int[] ret = new int[numF];

    float x,y,z;

    for(int f=0; f<numF; f++)

      {

      int[] faceI = indices[f];

      x = y = z = 0;

      for(int i : faceI)

        {

        float[] vertex = vertices[i];

        x += vertex[0];

        y += vertex[1];

        z += vertex[2];

        }

      int numV = faceI.length;

      x /= numV;

      y /= numV;

      z /= numV;

      x += position[0];

      y += position[1];

      z += position[2];

      ret[f] = isInFaces(faceAxis,dist3D, x,y,z) ? 0 : 1;

      }

    return ret;

    }

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

// return a table of vertices.length ints.

// Nth int is 0 if vertices[N] lies in a corner or edge of the shape; -1 otherwise.

  public static int[] computeVertexEffectsIndices(float[][] vertices, float[] position, Static3D[] faceAxis, float[] dist3D)

    {

    int numV = vertices.length;

    int[] ret = new int[numV];

    for(int v=0; v<numV; v++)

      {

      float[] vert = vertices[v];

      float x = vert[0] + position[0];

      float y = vert[1] + position[1];

      float z = vert[2] + position[2];

      ret[v] = numberInFaces(faceAxis,dist3D,x,y,z)>=2 ? 0 : -1;

      }

    return ret;

    }

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

// 1. for every potentialVertex, if it is on the same side of every plane present as the

//    'internalPoint', it must be one of the vertices of the ObjectShape. Remember which planes it

//    belongs to and add it to the list of vertices.

// 2. Having complete list of vertices and a bitmap of which planes each vertex belongs to, for each

//    plane:

//    a) assemble the list of vertices that belong to this plane

//    b) compute their center of mass and using this as a pivot, arrange the list into CCW order

//    c) out of the vertices and the CCW'd indices, construct an ObjectShape.

  public static ObjectShape createShape(float[][] cutPlanes, float[][] potentialVertices, float[] position, float[] internalPoint)

    {

    int numV = potentialVertices.length;

    float[][] tmp = new float[numV][];

    int numVertices = 0;

    for( float[] vert : potentialVertices )

      {

      boolean ok = true;

      for(float[] plane : cutPlanes)

        {

        if( isOnDifferentSides(internalPoint, vert, plane) )

          {

          ok = false;

          break;

          }

        }

      if( ok )

        {

        tmp[numVertices] = vert;

        numVertices++;

        }

      }

    float[][] vertices = new float[numVertices][];

    for(int v=0; v<numVertices; v++)

      {

      float[] t = tmp[v];

      vertices[v] = new float[] { t[0],t[1],t[2] };

      }

    int numP = cutPlanes.length;

    int[] numVerticesOnPlane = new int[numP];

    int[][] verticesOnPlane = new int[numP][numVertices];

    for(int p=0; p<numP; p++)

      {

      float[] plane = cutPlanes[p];

      for(int v=0; v<numVertices; v++)

        {

        float[] vert = vertices[v];

        if( vertexOnPlane(vert,plane) )

          {

          verticesOnPlane[p][numVerticesOnPlane[p]] = v;

          numVerticesOnPlane[p]++;

          }

        }

      }

    int numFaces = 0;

    for(int p=0; p<numP; p++)

      if( numVerticesOnPlane[p]>=3 ) numFaces++;

    int[][] indices = new int[numFaces][];

    int index = 0;

    for(int p=0; p<numP; p++ )

      if( numVerticesOnPlane[p]>=3 )

        {

        indices[index] = arrangeCCW(cutPlanes[p],internalPoint,vertices,numVerticesOnPlane[p],verticesOnPlane[p]);

        index++;

        }

    for( float[] v: vertices )

      {

      v[0] -= position[0];

      v[1] -= position[1];

      v[2] -= position[2];

      }

    return new ObjectShape(vertices,indices);

    }

  }