commit 7aa4c349539ed9e4f4e5c9b04a07fd4a5d003b44 Author: Leszek Koltunski Date: Fri Jul 9 14:42:26 2021 +0200 Progress with RubikControl. diff --git a/src/main/java/org/distorted/control/RubikControl.java b/src/main/java/org/distorted/control/RubikControl.java index 768c4f27..87ae6b5c 100644 --- a/src/main/java/org/distorted/control/RubikControl.java +++ b/src/main/java/org/distorted/control/RubikControl.java @@ -23,6 +23,7 @@ import org.distorted.helpers.BlockController; import org.distorted.library.main.DistortedNode; import org.distorted.library.main.DistortedScreen; import org.distorted.library.message.EffectListener; +import org.distorted.library.type.Static4D; import org.distorted.main.RubikActivity; import org.distorted.main.RubikSurfaceView; import org.distorted.objects.TwistyObject; @@ -57,6 +58,14 @@ public class RubikControl implements EffectListener return act!=null ? act.getObject() : null; } +/////////////////////////////////////////////////////////////////////////////////////////////////// + + Static4D getCurrQuat() + { + RubikActivity act = mRefAct.get(); + return act!=null ? act.getCurrQuat() : null; + } + /////////////////////////////////////////////////////////////////////////////////////////////////// RubikSurfaceView getSurfaceView() diff --git a/src/main/java/org/distorted/control/RubikControlRotate.java b/src/main/java/org/distorted/control/RubikControlRotate.java index f162b2e1..a7357fc5 100644 --- a/src/main/java/org/distorted/control/RubikControlRotate.java +++ b/src/main/java/org/distorted/control/RubikControlRotate.java @@ -55,8 +55,11 @@ class RubikControlRotate private void computeInitQuat() { - final double alphaZ = Math.PI/8; - final double alphaY = Math.PI/16; + double alphaZ = -Math.PI* 0.1250f; + double alphaY = -Math.PI* 0.0625f; + + alphaY /= 2; + alphaZ /= 2; float sinZ = (float)Math.sin(alphaZ); float cosZ = (float)Math.cos(alphaZ); @@ -70,10 +73,15 @@ class RubikControlRotate } /////////////////////////////////////////////////////////////////////////////////////////////////// +// Take 3D vector 'ax', rotate it by quaternion 'objQuat' to get vector V1. +// Take 3D vector (1,0,0) and rotate it by INIT_QUAT to get vector V2. +// Return a quaternion Q such that if we rotate V1 by Q, we get V2. - private Static4D computeQuat(Static3D ax) + private Static4D computeQuat(Static3D ax, Static4D objQuat, float x, float y, float z) { - return null; + Static4D ax4D = new Static4D( ax.get0(), ax.get1(), ax.get2(), 0); + Static4D axRo = QuatHelper.rotateVectorByQuat(ax4D,objQuat); + return QuatHelper.retRotationQuat(axRo.get0(),axRo.get1(),axRo.get2(),x,y,z); } /////////////////////////////////////////////////////////////////////////////////////////////////// @@ -82,20 +90,32 @@ class RubikControlRotate { TwistyObject object = mControl.getObject(); Static3D[] axis = object.getRotationAxis(); + int chosen=-1,numAxis = axis.length; float cos,maxCos = -1.0f; Static4D quat; - for (Static3D axi : axis) + Static4D objCurrQuat = mControl.getCurrQuat(); + Static4D axisX = new Static4D(1,0,0,0); + Static4D rotAxisX = QuatHelper.rotateVectorByQuat(axisX,INIT_QUAT); + + float axX = rotAxisX.get0(); + float axY = rotAxisX.get1(); + float axZ = rotAxisX.get2(); + + for (int a=0; a maxCos) { maxCos = cos; + chosen = a; mObjRotQuat = quat; } } + + android.util.Log.e("D", "axis chosen: "+chosen); } /////////////////////////////////////////////////////////////////////////////////////////////////// @@ -110,7 +130,8 @@ class RubikControlRotate private void setObjectEffectsStage1() { - // TODO + mDynamic.resetToBeginning(); + mScale.notifyWhenFinished(mControl); } /////////////////////////////////////////////////////////////////////////////////////////////////// @@ -186,11 +207,9 @@ class RubikControlRotate mObjectQuad = new MeshQuad(); } - if( INIT_QUAT==null ) - { - computeInitQuat(); - computeRotQuat(); - } + if( INIT_QUAT==null ) computeInitQuat(); + + computeRotQuat(); DistortedTexture texture = new DistortedTexture(); texture.setColorARGB(0xff00ff00); diff --git a/src/main/java/org/distorted/helpers/FactoryCubit.java b/src/main/java/org/distorted/helpers/FactoryCubit.java index b8a91632..e1d95d9b 100644 --- a/src/main/java/org/distorted/helpers/FactoryCubit.java +++ b/src/main/java/org/distorted/helpers/FactoryCubit.java @@ -368,29 +368,6 @@ public class FactoryCubit return ft; } -/////////////////////////////////////////////////////////////////////////////////////////////////// - - private double computeCos(double oldX, double oldY, double newX, double newY, double len1, double len2) - { - double ret= (oldX*newX+oldY*newY) / (len1*len2); - if( ret<-1.0 ) return -1.0; - if( ret> 1.0 ) return 1.0; - - return ret; - } - -/////////////////////////////////////////////////////////////////////////////////////////////////// -// sin of (signed!) angle between vectors 'old' and 'new', counterclockwise! - - private double computeSin(double oldX, double oldY, double newX, double newY, double len1, double len2) - { - double ret= (newX*oldY-oldX*newY) / (len1*len2); - if( ret<-1.0 ) return -1.0; - if( ret> 1.0 ) return 1.0; - - return ret; - } - /////////////////////////////////////////////////////////////////////////////////////////////////// private void rotateAllVertices(double[] result, int len, double[] vertices, double sin, double cos) @@ -550,8 +527,8 @@ public class FactoryCubit double newX = newVert[2*vertex ]; double newY = newVert[2*vertex+1]; double lenIthNew = Math.sqrt(newX*newX + newY*newY); - double cos = computeCos( oldVert[0], oldVert[1], newX, newY, lenIthNew, lenFirstOld); - double sin = computeSin( oldVert[0], oldVert[1], newX, newY, lenIthNew, lenFirstOld); + double cos = QuatHelper.computeCos( oldVert[0], oldVert[1], newX, newY, lenIthNew, lenFirstOld); + double sin = QuatHelper.computeSin( oldVert[0], oldVert[1], newX, newY, lenIthNew, lenFirstOld); rotateAllVertices(buffer,len,newVert,sin,cos); diff --git a/src/main/java/org/distorted/helpers/QuatHelper.java b/src/main/java/org/distorted/helpers/QuatHelper.java index d9c5e809..45a76f84 100644 --- a/src/main/java/org/distorted/helpers/QuatHelper.java +++ b/src/main/java/org/distorted/helpers/QuatHelper.java @@ -28,82 +28,136 @@ public class QuatHelper /////////////////////////////////////////////////////////////////////////////////////////////////// // return quat1*quat2 - public static Static4D quatMultiply( Static4D quat1, Static4D quat2 ) - { - float qx = quat1.get0(); - float qy = quat1.get1(); - float qz = quat1.get2(); - float qw = quat1.get3(); - - float rx = quat2.get0(); - float ry = quat2.get1(); - float rz = quat2.get2(); - float rw = quat2.get3(); - - float tx = rw*qx - rz*qy + ry*qz + rx*qw; - float ty = rw*qy + rz*qx + ry*qw - rx*qz; - float tz = rw*qz + rz*qw - ry*qx + rx*qy; - float tw = rw*qw - rz*qz - ry*qy - rx*qx; - - return new Static4D(tx,ty,tz,tw); - } + public static Static4D quatMultiply( Static4D quat1, Static4D quat2 ) + { + float qx = quat1.get0(); + float qy = quat1.get1(); + float qz = quat1.get2(); + float qw = quat1.get3(); + + float rx = quat2.get0(); + float ry = quat2.get1(); + float rz = quat2.get2(); + float rw = quat2.get3(); + + float tx = rw*qx - rz*qy + ry*qz + rx*qw; + float ty = rw*qy + rz*qx + ry*qw - rx*qz; + float tz = rw*qz + rz*qw - ry*qx + rx*qy; + float tw = rw*qw - rz*qz - ry*qy - rx*qx; + + return new Static4D(tx,ty,tz,tw); + } /////////////////////////////////////////////////////////////////////////////////////////////////// // rotate 'vector' by quat ( i.e. return quat*vector*(quat^-1) ) - public static Static4D rotateVectorByQuat(Static4D vector, Static4D quat) - { - float qx = quat.get0(); - float qy = quat.get1(); - float qz = quat.get2(); - float qw = quat.get3(); + public static Static4D rotateVectorByQuat(Static4D vector, Static4D quat) + { + float qx = quat.get0(); + float qy = quat.get1(); + float qz = quat.get2(); + float qw = quat.get3(); - Static4D quatInverted= new Static4D(-qx,-qy,-qz,qw); - Static4D tmp = quatMultiply(quat,vector); + Static4D quatInverted= new Static4D(-qx,-qy,-qz,qw); + Static4D tmp = quatMultiply(quat,vector); - return quatMultiply(tmp,quatInverted); - } + return quatMultiply(tmp,quatInverted); + } /////////////////////////////////////////////////////////////////////////////////////////////////// // rotate 'vector' by quat^(-1) ( i.e. return (quat^-1)*vector*quat ) - public static Static4D rotateVectorByInvertedQuat(Static4D vector, Static4D quat) + public static Static4D rotateVectorByInvertedQuat(Static4D vector, Static4D quat) + { + float qx = quat.get0(); + float qy = quat.get1(); + float qz = quat.get2(); + float qw = quat.get3(); + + Static4D quatInverted= new Static4D(-qx,-qy,-qz,qw); + Static4D tmp = quatMultiply(quatInverted,vector); + + return quatMultiply(tmp,quat); + } + +/////////////////////////////////////////////////////////////////////////////////////////////////// + + public static Static4D quatFromDrag(float dragX, float dragY) + { + float axisX = dragY; // inverted X and Y - rotation axis is perpendicular to (dragX,dragY) + float axisY = dragX; // Why not (-dragY, dragX) ? because Y axis is also inverted! + float axisZ = 0; + float axisL = (float)Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ); + + if( axisL>0 ) { - float qx = quat.get0(); - float qy = quat.get1(); - float qz = quat.get2(); - float qw = quat.get3(); + axisX /= axisL; + axisY /= axisL; + axisZ /= axisL; + + float ratio = axisL; + ratio = ratio - (int)ratio; // the cos() is only valid in (0,Pi) - Static4D quatInverted= new Static4D(-qx,-qy,-qz,qw); - Static4D tmp = quatMultiply(quatInverted,vector); + float cosA = (float)Math.cos(Math.PI*ratio); + float sinA = (float)Math.sqrt(1-cosA*cosA); - return quatMultiply(tmp,quat); + return new Static4D(axisX*sinA, axisY*sinA, axisZ*sinA, cosA); } + return new Static4D(0f, 0f, 0f, 1f); + } + /////////////////////////////////////////////////////////////////////////////////////////////////// - public static Static4D quatFromDrag(float dragX, float dragY) - { - float axisX = dragY; // inverted X and Y - rotation axis is perpendicular to (dragX,dragY) - float axisY = dragX; // Why not (-dragY, dragX) ? because Y axis is also inverted! - float axisZ = 0; - float axisL = (float)Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ); + public static double computeCos(double oldX, double oldY, double newX, double newY, double len1, double len2) + { + double ret= (oldX*newX+oldY*newY) / (len1*len2); + if( ret<-1.0 ) return -1.0; + if( ret> 1.0 ) return 1.0; + + return ret; + } + +/////////////////////////////////////////////////////////////////////////////////////////////////// +// sin of (signed!) angle between vectors 'old' and 'new', counterclockwise! - if( axisL>0 ) - { - axisX /= axisL; - axisY /= axisL; - axisZ /= axisL; + public static double computeSin(double oldX, double oldY, double newX, double newY, double len1, double len2) + { + double ret= (newX*oldY-oldX*newY) / (len1*len2); + if( ret<-1.0 ) return -1.0; + if( ret> 1.0 ) return 1.0; - float ratio = axisL; - ratio = ratio - (int)ratio; // the cos() is only valid in (0,Pi) + return ret; + } - float cosA = (float)Math.cos(Math.PI*ratio); - float sinA = (float)Math.sqrt(1-cosA*cosA); +/////////////////////////////////////////////////////////////////////////////////////////////////// +// return quat Q that turns 3D vector A=(ax,ay,az) to another 3D vector B=(bx,by,bz) +// take care of double-cover by ensuring that always Q.get3() >=0 + + public static Static4D retRotationQuat(float ax, float ay, float az, float bx, float by, float bz) + { + float nx = ay*bz - az*by; + float ny = az*bx - ax*bz; + float nz = ax*by - ay*bx; - return new Static4D(axisX*sinA, axisY*sinA, axisZ*sinA, cosA); - } + float sin = (float)Math.sqrt(nx*nx + ny*ny + nz*nz); + float cos = ax*bx + ay*by + az*bz; - return new Static4D(0f, 0f, 0f, 1f); + if( sin!=0 ) + { + nx /= sin; + ny /= sin; + nz /= sin; } + + // Why sin<=0 and cos>=0 ? + // 0 0 both sin and cos are positive. + // But1: quats work counterclockwise -> negate cos. + // But2: double-cover, we prefer to have the cos positive (so that unit=(0,0,0,1)) + // so negate again both cos and sin. + float sinHalf =-(float)Math.sqrt((1-cos)/2); + float cosHalf = (float)Math.sqrt((1+cos)/2); + + return new Static4D(nx*sinHalf,ny*sinHalf,nz*sinHalf,cosHalf); + } } diff --git a/src/main/java/org/distorted/main/RubikActivity.java b/src/main/java/org/distorted/main/RubikActivity.java index 1b5e16cd..0855dc52 100644 --- a/src/main/java/org/distorted/main/RubikActivity.java +++ b/src/main/java/org/distorted/main/RubikActivity.java @@ -44,6 +44,7 @@ import org.distorted.helpers.TwistyPreRender; import org.distorted.library.main.DistortedLibrary; import org.distorted.library.main.DistortedScreen; +import org.distorted.library.type.Static4D; import org.distorted.objects.TwistyObject; import org.distorted.network.RubikScores; import org.distorted.network.RubikNetwork; @@ -602,6 +603,14 @@ public class RubikActivity extends TwistyActivity play.setLockState(this); } +/////////////////////////////////////////////////////////////////////////////////////////////////// + + public Static4D getCurrQuat() + { + RubikSurfaceView view = findViewById(R.id.rubikSurfaceView); + return view.getQuat(); + } + /////////////////////////////////////////////////////////////////////////////////////////////////// public void switchTutorial(String url, ObjectList object, int size)