Last active
July 8, 2020 07:21
-
-
Save d3x0r/9ffea1d55f079b8ce4d958ddf0ad6d0c to your computer and use it in GitHub Desktop.
Revisions
-
d3x0r revised this gist
Jul 8, 2020 . 1 changed file with 68 additions and 8 deletions.There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode charactersOriginal file line number Diff line number Diff line change @@ -10,14 +10,14 @@ This is the base form... the steps will be broken out for phases of substitution As implemented here https://github.com/d3x0r/STFRPhysics/blob/master/3d/src/dual-quat.js#L347 ``` js const q = { x: AngleX, y:AngleY, z:AngleZ } ; ``` Compute the total rotation of the system (angle is sum of angles). If there is no rotation, return the default basis. Also compute a square normal; take the reciprical for multiplation later instead of division. ``` js const nt = (Math.abs(q.x)+Math.abs(q.y)+Math.abs(q.z)) / 2; if( !nt ) { return {forward:{x:0,y:0,z:1}, right:{x:1,y:0,z:0}, up:{x:0,y:1,z:0}, origin:{x:0,y:0,z:0 }}; @@ -28,23 +28,23 @@ Also compute a square normal; take the reciprical for multiplation later instead Use the total angle, and get the sin/cos of that angle; a quaternion has this already built into its factors. ``` js const s = Math.sin( del * nt ); // sin/cos are the function of exp() const qw = Math.cos( del * nt ); // sin/cos are the function of exp() ``` Normalize the inputs, and multiply by the sin of the angle/2; this is already a factor of a quaternion, and q.w would be the cos above... ``` js const qx = q.x * nR * s; // normalizes the imaginary parts const qy = q.y * nR * s; // set the sin of their composite angle as their total const qz = q.z * nR * s; // output = 1(unit vector) * sin in x,y,z parts. ``` Finally; these are all of the common factors. ``` js const xy = 2*qx*qy; // sin(t)*sin(t) * x * y / (xx+yy+zz) const yz = 2*qy*qz; // sin(t)*sin(t) * y * z / (xx+yy+zz) const xz = 2*qx*qz; // sin(t)*sin(t) * x * z / (xx+yy+zz) @@ -61,7 +61,7 @@ Finally; these are all of the common factors. Which can be used to create a matrix like this. ``` js const basis = { right :{ x : 1 - ( yy + zz ), y : ( wz + xy ), z : ( xz - wy ) } , up :{ x : ( xy - wz ), y : 1 - ( zz + xx ), z : ( wx + yz ) } , forward:{ x : ( wy + xz ), y : ( yz - wx ), z : 1 - ( xx + yy ) } @@ -75,7 +75,7 @@ Which can be used to create a matrix like this. Update() does the 'heavy' work, the sin/cos lookup, sqrt normal calculation.... ``` js lnQuat.prototype.update = function() { // sqrt, 3 mul 2 add 1 div 1 sin 1 cos if( !this.dirty ) return; @@ -106,7 +106,7 @@ lnQuat.prototype.update = function() { And then Apply() takes a vector(v) and applys the log-quaternion rotation to it.... ``` js // https://blog.molecular-matters.com/2013/05/24/a-faster-quaternion-vector-multiplication/ // @@ -135,3 +135,63 @@ lnQuat.prototype.apply = function( v ) { , y : v.y + qw * ty + ( qz * tx - tz * qx ) , z : v.z + qw * tz + ( qx * ty - tx * qy ) }; } ``` ## Reversing Matrix to Angles The trace of the matrix gives the angle. ``` js lnQuat.prototype.fromBasis = function( basis ) { // tr(M)=2cos(theta)+1 . const t = ( ( basis.right.x + basis.up.y + basis.forward.z ) - 1 )/2; let angle = acos(t); if( !angle ) { this.x = this.y = this.z = this.nx = this.ny = this.nz = this.nL = this.nR = 0; this.ny = 1; // axis normal. this.s = 0; // sin(angle) this.qw = 1; // cos(angle) return this; // set to 0. } /* from another page (missing reference) x = (R21 - R12)/sqrt((R21 - R12)^2+(R02 - R20)^2+(R10 - R01)^2); y = (R02 - R20)/sqrt((R21 - R12)^2+(R02 - R20)^2+(R10 - R01)^2); z = (R10 - R01)/sqrt((R21 - R12)^2+(R02 - R20)^2+(R10 - R01)^2); */ // Recommend: Add +/- 2PI occasionally, since any +/- n* 2PI // rotation is this same matrix, but we don't really know which this came from... const yz = basis.up .z - basis.forward.y; const xz = basis.forward.x - basis.right .z; const xy = basis.right .y - basis.up .x; const tmp = 1 /Math.sqrt( yz*yz + xz*xz + xy*xy ); this.nx = yz *tmp; this.ny = xz *tmp; this.nz = xy *tmp; const lNorm = angle / (Math.abs(this.nx)+Math.abs(this.ny)+Math.abs(this.nz)); this.x = this.nx * lNorm; this.y = this.ny * lNorm; this.z = this.nz * lNorm; return this; } ``` ## arccos > 1 did a lot of research leading up to this, and this works for arccos(cos(a)+cos(b))... ``` js // 'fixed' acos for inputs > 1 function acos(x) { const mod = (x,y)=>y * (x / y - Math.floor(x / y)) ; const plusminus = (x)=>mod( x+1,2)-1; const trunc = (x,y)=>x-mod(x,y); return Math.acos(plusminus(x)); } ``` -
Jul 8, 2020 .There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode charactersOriginal file line number Diff line number Diff line change @@ -0,0 +1,137 @@ This is an alternative method to get a basis from a quaternion (log quaternion)... or from angle/angle/angle. This is built by rotating the constant vectors (1,0,0), (0,1,0), (0,0,1) via standard quaternion rotation, and reducing common factors. The `0`s and `1`s collapse out a lot of the terms of apply. It ends up being less work to get the 3 vector basis than to rotate a single point; although if you USE the basis to multiply with the point; that increases the work to excess of just rotating a vector directly... This is the base form... the steps will be broken out for phases of substitution. As implemented here https://github.com/d3x0r/STFRPhysics/blob/master/3d/src/dual-quat.js#L347 ``` const q = { x: AngleX, y:AngleY, z:AngleZ } ; ``` Compute the total rotation of the system (angle is sum of angles). If there is no rotation, return the default basis. Also compute a square normal; take the reciprical for multiplation later instead of division. ``` const nt = (Math.abs(q.x)+Math.abs(q.y)+Math.abs(q.z)) / 2; if( !nt ) { return {forward:{x:0,y:0,z:1}, right:{x:1,y:0,z:0}, up:{x:0,y:1,z:0}, origin:{x:0,y:0,z:0 }}; } const nR = 1/Math.sqrt( q.x*q.x + q.y*q.y + q.z*q.z ); ``` Use the total angle, and get the sin/cos of that angle; a quaternion has this already built into its factors. ``` const s = Math.sin( del * nt ); // sin/cos are the function of exp() const qw = Math.cos( del * nt ); // sin/cos are the function of exp() ``` Normalize the inputs, and multiply by the sin of the angle/2; this is already a factor of a quaternion, and q.w would be the cos above... ``` const qx = q.x * nR * s; // normalizes the imaginary parts const qy = q.y * nR * s; // set the sin of their composite angle as their total const qz = q.z * nR * s; // output = 1(unit vector) * sin in x,y,z parts. ``` Finally; these are all of the common factors. ``` const xy = 2*qx*qy; // sin(t)*sin(t) * x * y / (xx+yy+zz) const yz = 2*qy*qz; // sin(t)*sin(t) * y * z / (xx+yy+zz) const xz = 2*qx*qz; // sin(t)*sin(t) * x * z / (xx+yy+zz) const wx = 2*qw*qx; // cos(t)*sin(t) * x / sqrt(xx+yy+zz) const wy = 2*qw*qy; // cos(t)*sin(t) * y / sqrt(xx+yy+zz) const wz = 2*qw*qz; // cos(t)*sin(t) * z / sqrt(xx+yy+zz) const xx = 2*qx*qx; // sin(t)*sin(t) * y * y / (xx+yy+zz) const yy = 2*qy*qy; // sin(t)*sin(t) * x * x / (xx+yy+zz) const zz = 2*qz*qz; // sin(t)*sin(t) * z * z / (xx+yy+zz) ``` Which can be used to create a matrix like this. ``` const basis = { right :{ x : 1 - ( yy + zz ), y : ( wz + xy ), z : ( xz - wy ) } , up :{ x : ( xy - wz ), y : 1 - ( zz + xx ), z : ( wx + yz ) } , forward:{ x : ( wy + xz ), y : ( yz - wx ), z : 1 - ( xx + yy ) } }; ``` ## Update/Apply Update() does the 'heavy' work, the sin/cos lookup, sqrt normal calculation.... ``` lnQuat.prototype.update = function() { // sqrt, 3 mul 2 add 1 div 1 sin 1 cos if( !this.dirty ) return; this.dirty = false; // norm-rect this.nR = Math.sqrt(this.x*this.x + this.y*this.y + this.z*this.z); // norm-linear this is / 3 usually, but the sine lookup would // adds a /3 back in which reverses it. this.nL = (Math.abs(this.x)+Math.abs(this.y)+Math.abs(this.z))/2;///(2*Math.PI); // average of total if( this.nR ){ this.nx = this.x/this.nR /* * this.nL*/; this.ny = this.y/this.nR /* * this.nL*/; this.nz = this.z/this.nR /* * this.nL*/; }else { this.nx = 0; this.ny = 0; this.nz = 0; } this.s = Math.sin(this.nL); // only want one half wave... 0-pi total. this.qw = Math.cos(this.nL); return this; } ``` And then Apply() takes a vector(v) and applys the log-quaternion rotation to it.... ``` // https://blog.molecular-matters.com/2013/05/24/a-faster-quaternion-vector-multiplication/ // lnQuat.prototype.apply = function( v ) { //return this.applyDel( v, 1.0 ); const q = this; this.update(); if( !q.nL ) { // v is unmodified. return {x:v.x, y:v.y, z:v.z }; // 1.0 } // q.s and q.qw are set in update(); they are constants for a quat in a location. const nst = q.s/this.nR; // normal * sin_theta const qw = q.qw; //Math.cos( pl ); quaternion q.w = (exp(lnQ)) [ *exp(lnQ.W=0) ] const qx = q.x*nst; const qy = q.y*nst; const qz = q.z*nst; //p’ = (v*v.dot(p) + v.cross(p)*(w))*2 + p*(w*w – v.dot(v)) const tx = 2 * (qy * v.z - qz * v.y); // v.cross(p)*w*2 const ty = 2 * (qz * v.x - qx * v.z); const tz = 2 * (qx * v.y - qy * v.x); return { x : v.x + qw * tx + ( qy * tz - ty * qz ) , y : v.y + qw * ty + ( qz * tx - tz * qx ) , z : v.z + qw * tz + ( qx * ty - tx * qy ) }; }