/*
* Based on a c++ template by Marc ten Bosch
* https://marctenbosch.com/news/2011/05/4d-rotations-and-the-4d-equivalent-of-quaternions/
*
* Rotor4 was founded on Marc ten Bosch's Rotor3
* https://marctenbosch.com/quaternions/code.htm
*
* Rotor equation derivations are explained in https://joesubbi.github.io/2022/04/04/4D-Rotation/
*/publicclassBivector4{publicfloatbxy=0;publicfloatbxz=0;publicfloatbyz=0;publicfloatbxw=0;publicfloatbyw=0;publicfloatbzw=0;publicBivector4(floatbxy,floatbxz,floatbyz,floatbxw,floatbyw,floatbzw){this.bxy=bxy;this.bxz=bxz;this.byz=byz;this.bxw=bxw;this.byw=byw;this.bzw=bzw;}publicstaticBivector4Wedge(Vector4u,Vector4v){Bivector4b=newBivector4(u.x*v.y-u.y*v.x,//XYu.x*v.z-u.z*v.x,//XZu.y*v.z-u.z*v.y,//YZu.x*v.w-u.w*v.x,//XWu.y*v.w-u.w*v.y,//YWu.z*v.w-u.w*v.z//ZW);returnb;}}publicclassRotor4{// scalar partpublicfloata=1;// bivector partpublicfloatbxy=0;publicfloatbxz=0;publicfloatbyz=0;publicfloatbxw=0;publicfloatbyw=0;publicfloatbzw=0;// pseudo-vector partpublicfloatpxyzw=0;privateconstfloatepsilon=1e-6f;privatestaticRotor4identity=newRotor4(1,0,0,0,0,0,0,0);// ---- CONSTRUCTORS ----publicRotor4(floata,floatbxy,floatbxz,floatbyz,floatbxw,floatbyw,floatbzw,floatbxyzw){this.a=a;this.bxy=bxy;this.bxz=bxz;this.byz=byz;this.bxw=bxw;this.byw=byw;this.bzw=bzw;this.pxyzw=bxyzw;}publicRotor4(Bivector4bv,floatangle){floatsina=Mathf.Sin(angle/2);a=Mathf.Cos(angle/2);bxy=-sina*bv.bxy;bxz=-sina*bv.bxz;byz=-sina*bv.byz;bxw=-sina*bv.bxw;byw=-sina*bv.byw;bzw=-sina*bv.bzw;pxyzw=0;Normalise();}publicstaticRotor4Identity(){returnidentity;}// ---- OPERATORS ----// Rotor4-Rotor4 ProductpublicstaticRotor4operator*(Rotor4a,Rotor4b){floate=-a.bxw*b.bxw-a.bxy*b.bxy-a.bxz*b.bxz-a.byw*b.byw-a.byz*b.byz-a.bzw*b.bzw+a.pxyzw*b.pxyzw+a.a*b.a;floatexy=-a.bxw*b.byw+a.bxy*b.a-a.bxz*b.byz+a.byw*b.bxw+a.byz*b.bxz-a.bzw*b.pxyzw-a.pxyzw*b.bzw+a.a*b.bxy;floatexz=-a.bxw*b.bzw+a.bxy*b.byz+a.bxz*b.a+a.byw*b.pxyzw-a.byz*b.bxy+a.bzw*b.bxw+a.pxyzw*b.byw+a.a*b.bxz;floatexw=a.bxw*b.a+a.bxy*b.byw+a.bxz*b.bzw-a.byw*b.bxy-a.byz*b.pxyzw-a.bzw*b.bxz-a.pxyzw*b.byz+a.a*b.bxw;floateyz=-a.bxw*b.pxyzw-a.bxy*b.bxz+a.bxz*b.bxy-a.byw*b.bzw+a.byz*b.a+a.bzw*b.byw-a.pxyzw*b.bxw+a.a*b.byz;floateyw=a.bxw*b.bxy-a.bxy*b.bxw+a.bxz*b.pxyzw+a.byw*b.a+a.byz*b.bzw-a.bzw*b.byz+a.pxyzw*b.bxz+a.a*b.byw;floatezw=a.bxw*b.bxz-a.bxy*b.pxyzw-a.bxz*b.bxw+a.byw*b.byz-a.byz*b.byw+a.bzw*b.a-a.pxyzw*b.bxy+a.a*b.bzw;floatexyzw=a.bxw*b.byz+a.bxy*b.bzw-a.bxz*b.byw-a.byw*b.bxz+a.byz*b.bxw+a.bzw*b.bxy+a.pxyzw*b.a+a.a*b.pxyzw;returnnewRotor4(e,exy,exz,eyz,exw,eyw,ezw,exyzw);}// Rotor4-Vector4 Product (Rotation)publicstaticVector4operator*(Rotor4r,Vector4a){returnr.Rotate(a);}publicstaticVector4operator*(Vector4a,Rotor4r){returnr.Rotate(a);}// Rotate a Vector with a RotorpublicVector4Rotate(Vector4a){floats=this.a;floats2=s*s;floatbxy2=bxy*bxy;floatbxz2=bxz*bxz;floatbxw2=bxw*bxw;floatbyz2=byz*byz;floatbyw2=byw*byw;floatbzw2=bzw*bzw;floatbxyzw2=pxyzw*pxyzw;Vector4r;r.x=(2*a.w*bxw*s+2*a.w*bxy*byw+2*a.w*bxz*bzw+2*a.w*byz*pxyzw-a.x*bxw2-a.x*bxy2-a.x*bxz2+a.x*byw2+a.x*byz2+a.x*bzw2-a.x*bxyzw2+a.x*s2-2*a.y*bxw*byw+2*a.y*bxy*s-2*a.y*bxz*byz+2*a.y*bzw*pxyzw-2*a.z*bxw*bzw+2*a.z*bxy*byz+2*a.z*bxz*s-2*a.z*byw*pxyzw);r.y=(-2*a.w*bxw*bxy-2*a.w*bxz*pxyzw+2*a.w*byw*s+2*a.w*byz*bzw-2*a.x*bxw*byw-2*a.x*bxy*s-2*a.x*bxz*byz-2*a.x*bzw*pxyzw+a.y*bxw2-a.y*bxy2+a.y*bxz2-a.y*byw2-a.y*byz2+a.y*bzw2-a.y*bxyzw2+a.y*s2+2*a.z*bxw*pxyzw-2*a.z*bxy*bxz-2*a.z*byw*bzw+2*a.z*byz*s);r.z=(-2*a.w*bxw*bxz+2*a.w*bxy*pxyzw-2*a.w*byw*byz+2*a.w*bzw*s-2*a.x*bxw*bzw+2*a.x*bxy*byz-2*a.x*bxz*s+2*a.x*byw*pxyzw-2*a.y*bxw*pxyzw-2*a.y*bxy*bxz-2*a.y*byw*bzw-2*a.y*byz*s+a.z*bxw2+a.z*bxy2-a.z*bxz2+a.z*byw2-a.z*byz2-a.z*bzw2-a.z*bxyzw2+a.z*s2);r.w=(-a.w*bxw2+a.w*bxy2+a.w*bxz2-a.w*byw2+a.w*byz2-a.w*bzw2-a.w*bxyzw2+a.w*s2-2*a.x*bxw*s+2*a.x*bxy*byw+2*a.x*bxz*bzw-2*a.x*byz*pxyzw-2*a.y*bxw*bxy+2*a.y*bxz*pxyzw-2*a.y*byw*s+2*a.y*byz*bzw-2*a.z*bxw*bxz-2*a.z*bxy*pxyzw-2*a.z*byw*byz-2*a.z*bzw*s);returnr;}// Rotate one Rotor to anotherpublicRotor4Rotate(Rotor4r){returnthis*r*this.Reverse();}// Spherical Linear Interpolation between two Rotors (MOSTLY WORKS - NOT PERFECT - FIX IN PROGRESS)publicstaticRotor4SLerp(Rotor4from,Rotor4to,floatratio,boolnormalise=true){// The following SLerp is from:// https://referencesource.microsoft.com/#System.Numerics/System/Numerics/Quaternion.csfloatt=ratio;floatsq(floatx)=>x*x;Rotor4difference=RotorFromTo(from,to);floatcosOmega=Mathf.Sqrt(sq(difference.a)+sq(difference.pxyzw));boolflip=false;if(cosOmega<0.0f){flip=true;cosOmega=-cosOmega;}floats1,s2;if(cosOmega>(1.0f-epsilon)){// Too close, do straight linear interpolation.s1=1.0f-t;s2=(flip)?-t:t;}else{floatomega=(float)Mathf.Acos(cosOmega);floatinvSinOmega=(float)(1/Mathf.Sin(omega));s1=(float)Mathf.Sin((1.0f-t)*omega)*invSinOmega;s2=(flip)?(float)-Mathf.Sin(t*omega)*invSinOmega:(float)Mathf.Sin(t*omega)*invSinOmega;}Rotor4toReturn=newRotor4(s1*from.a+s2*to.a,s1*from.bxy+s2*to.bxy,s1*from.bxz+s2*to.bxz,s1*from.byz+s2*to.byz,s1*from.bxw+s2*to.bxw,s1*from.byw+s2*to.byw,s1*from.bzw+s2*to.bzw,s1*from.pxyzw+s2*to.pxyzw);if(normalise)toReturn.Normalise();returntoReturn;// Get the angle between rotors// create a rotor, but with the angle scaled by t//Rotor4 toReturn = RotorFromTo(from, to);}// ---- ROTOR OPERATIONS ----// ConjugatepublicRotor4Reverse(){returnnewRotor4(a,-bxy,-bxz,-byz,-bxw,-byw,-bzw,pxyzw);}// Length SquaredprivatefloatLengthSquared(){floatsq(floatx)=>x*x;returnsq(a)+sq(bxy)+sq(bxz)+sq(byz)+sq(bxw)+sq(byw)+sq(bzw)+sq(pxyzw);}// LengthprivatefloatLength(){returnMathf.Sqrt(LengthSquared());}// Normalise this RotorpublicvoidNormalise(){floatl=Length();a/=l;bxy/=l;bxz/=l;byz/=l;bxw/=l;byw/=l;bzw/=l;pxyzw/=l;}// Normalised copy of a RotorpublicRotor4Normal(){Rotor4r=this;r.Normalise();returnr;}// The Rotor to rotate one Rotor to anotherpublicstaticRotor4RotorFromTo(Rotor4a,Rotor4b){Rotor4difference=b*a.Reverse();difference.Normalise();returndifference;}// The Angle between two Rotors (radians)publicstaticfloatAngleFromTo(Rotor4a,Rotor4b){floatsq(floatx)=>x*x;Rotor4difference=RotorFromTo(a,b);returnMathf.Acos(Mathf.Sqrt(sq(difference.a)+sq(difference.pxyzw)))*2;}// The Rotor to get from a to b, and the angle between rotorpublicstatic(Rotor4,float)Difference(Rotor4a,Rotor4b){Rotor4difference=RotorFromTo(a,b);floatangle=AngleFromTo(a,b);return(difference,angle);}// Dot Product of two RotorspublicstaticRotor4Dot(Rotor4a,Rotor4b){floate=-a.bxw*b.bxw-a.bxy*b.bxy-a.bxz*b.bxz-a.byw*b.byw-a.byz*b.byz-a.bzw*b.bzw+a.pxyzw-b.pxyzw;floatexy=-a.bzw*b.pxyzw-a.pxyzw*b.bzw;floatexz=a.byw*b.pxyzw+a.pxyzw*b.byw;floatexw=-a.byz*b.pxyzw+a.pxyzw*b.byz;floateyz=-a.bxw*b.pxyzw+a.pxyzw*b.bxw;floateyw=a.bxz*b.pxyzw+a.pxyzw*b.bxz;floatezw=-a.bxy*b.pxyzw+a.pxyzw*b.bxy;floatexyzw=0;returnnewRotor4(e,exy,exz,eyz,exw,eyw,ezw,exyzw);}// Geometric ProductpublicstaticRotor4GeoProd(Vector4a,Vector4b){Bivector4bv=Bivector4.Wedge(a,b);returnnewRotor4(Vector4.Dot(a,b),bv.bxy,bv.bxz,bv.byz,bv.bxw,bv.byw,bv.bzw,0);}// ---- EQUALITY OPERATIONS ----publicstaticboolExactlyEqual(Rotor4a,Rotor4b){if(a.a==b.a&&a.byz==b.byz&&a.bxz==b.bxz&&a.bxy==b.bxy&&a.bxw==b.bxw&&a.byw==b.byw&&a.bzw==b.bzw)returntrue;returnfalse;}publicstaticboolApproxEqual(Rotor4a,Rotor4b,floate=0.005f){floatangle=AngleFromTo(a,b);returnangle<e;}publicstaticbooloperator==(Rotor4lhs,Rotor4rhs){returnApproxEqual(lhs,rhs);}publicstaticbooloperator!=(Rotor4lhs,Rotor4rhs){// Returns true in the presence of NaN values.return!(lhs==rhs);}publicboolEquals(Rotor4rotor){returnthis==rotor;}publicoverrideboolEquals(objectobj)=>Equals(objasRotor4);publicoverrideintGetHashCode(){returnHashCode.Combine(a,bxy,bxz,byz,bxw,byw,bzw,pxyzw);}// ---- Utility Functions ----publicoverridestringToString(){returna+" + "+bxy+" + "+bxz+" + "+byz+" + "+bxw+" + "+byw+" + "+bzw+" + "+pxyzw;}publicfloat[]ToArray(){returnnewfloat[]{a,bxy,bxz,byz,bxw,byw,bzw,pxyzw};}}