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 0s and 1s 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() 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 ) };
}The trace of the matrix gives the angle.
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;
}did a lot of research leading up to this, and this works for arccos(cos(a)+cos(b))...
// '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));
}