commit 8fc3b80a5ec95d036327b255f899ae85ab3200b5 Author: Leszek Koltunski Date: Thu Sep 30 16:03:21 2021 +0200 Move QuatHelpeer to distorted-library diff --git a/src/main/java/org/distorted/library/main/QuatHelper.java b/src/main/java/org/distorted/library/main/QuatHelper.java new file mode 100644 index 0000000..8a305ac --- /dev/null +++ b/src/main/java/org/distorted/library/main/QuatHelper.java @@ -0,0 +1,163 @@ +/////////////////////////////////////////////////////////////////////////////////////////////////// +// Copyright 2021 Leszek Koltunski // +// // +// This file is part of Magic Cube. // +// // +// Magic Cube is free software: you can redistribute it and/or modify // +// it under the terms of the GNU General Public License as published by // +// the Free Software Foundation, either version 2 of the License, or // +// (at your option) any later version. // +// // +// Magic Cube is distributed in the hope that it will be useful, // +// but WITHOUT ANY WARRANTY; without even the implied warranty of // +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // +// GNU General Public License for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with Magic Cube. If not, see . // +/////////////////////////////////////////////////////////////////////////////////////////////////// + +package org.distorted.library.main; + +import org.distorted.library.type.Static4D; + +/////////////////////////////////////////////////////////////////////////////////////////////////// + +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); + } + +/////////////////////////////////////////////////////////////////////////////////////////////////// +// 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(); + + Static4D quatInverted= new Static4D(-qx,-qy,-qz,qw); + Static4D tmp = quatMultiply(quat,vector); + + return quatMultiply(tmp,quatInverted); + } + +/////////////////////////////////////////////////////////////////////////////////////////////////// +// rotate 'vector' by quat^(-1) ( i.e. return (quat^-1)*vector*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 ) + { + axisX /= axisL; + axisY /= axisL; + axisZ /= axisL; + + float ratio = axisL; + ratio = ratio - (int)ratio; // the cos() is only valid in (0,Pi) + + float cosA = (float)Math.cos(Math.PI*ratio); + float sinA = (float)Math.sqrt(1-cosA*cosA); + + return new Static4D(axisX*sinA, axisY*sinA, axisZ*sinA, cosA); + } + + return new Static4D(0f, 0f, 0f, 1f); + } + +/////////////////////////////////////////////////////////////////////////////////////////////////// + + 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! + + 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; + + return ret; + } + +/////////////////////////////////////////////////////////////////////////////////////////////////// +// 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; + + float sin = (float)Math.sqrt(nx*nx + ny*ny + nz*nz); + float cos = ax*bx + ay*by + az*bz; + + 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); + } + }