/*-
* #%L
* Fiji distribution of ImageJ for the life sciences.
* %%
* Copyright (C) 2007 - 2017 Fiji developers.
* %%
* This program 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.
*
* This program 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 this program. If not, see
* <http://www.gnu.org/licenses/gpl-2.0.html>.
* #L%
*/
package spim.vecmath;
/*
* Copyright 1997-2008 Sun Microsystems, Inc. All Rights Reserved.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* This code is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License version 2 only, as
* published by the Free Software Foundation. Sun designates this
* particular file as subject to the "Classpath" exception as provided
* by Sun in the LICENSE file that accompanied this code.
*
* This code 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
* version 2 for more details (a copy is included in the LICENSE file that
* accompanied this code).
*
* You should have received a copy of the GNU General Public License version
* 2 along with this work; if not, write to the Free Software Foundation,
* Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
*
* Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
* CA 95054 USA or visit www.sun.com if you need additional information or
* have any questions.
*
*/
/**
* A 4 element unit quaternion represented by single precision floating point
* x,y,z,w coordinates. The quaternion is always normalized.
*
*/
public class Quat4f extends Tuple4f implements java.io.Serializable
{
// Combatible with 1.1
static final long serialVersionUID = 2675933778405442383L;
final static double EPS = 0.000001;
final static double EPS2 = 1.0e-30;
final static double PIO2 = 1.57079632679;
/**
* Constructs and initializes a Quat4f from the specified xyzw coordinates.
*
* @param x
* the x coordinate
* @param y
* the y coordinate
* @param z
* the z coordinate
* @param w
* the w scalar component
*/
public Quat4f( float x, float y, float z, float w )
{
float mag;
mag = (float) ( 1.0 / Math.sqrt( x * x + y * y + z * z + w * w ) );
this.x = x * mag;
this.y = y * mag;
this.z = z * mag;
this.w = w * mag;
}
/**
* Constructs and initializes a Quat4f from the array of length 4.
*
* @param q
* the array of length 4 containing xyzw in order
*/
public Quat4f( float[] q )
{
float mag;
mag = (float) ( 1.0 / Math.sqrt( q[ 0 ] * q[ 0 ] + q[ 1 ] * q[ 1 ]
+ q[ 2 ] * q[ 2 ] + q[ 3 ] * q[ 3 ] ) );
x = q[ 0 ] * mag;
y = q[ 1 ] * mag;
z = q[ 2 ] * mag;
w = q[ 3 ] * mag;
}
/**
* Constructs and initializes a Quat4f from the specified Quat4f.
*
* @param q1
* the Quat4f containing the initialization x y z w data
*/
public Quat4f( Quat4f q1 )
{
super( q1 );
}
/**
* Constructs and initializes a Quat4f from the specified Quat4d.
*
* @param q1
* the Quat4d containing the initialization x y z w data
*/
public Quat4f( Quat4d q1 )
{
super( q1 );
}
/**
* Constructs and initializes a Quat4f from the specified Tuple4f.
*
* @param t1
* the Tuple4f containing the initialization x y z w data
*/
public Quat4f( Tuple4f t1 )
{
float mag;
mag = (float) ( 1.0 / Math.sqrt( t1.x * t1.x + t1.y * t1.y + t1.z
* t1.z + t1.w * t1.w ) );
x = t1.x * mag;
y = t1.y * mag;
z = t1.z * mag;
w = t1.w * mag;
}
/**
* Constructs and initializes a Quat4f from the specified Tuple4d.
*
* @param t1
* the Tuple4d containing the initialization x y z w data
*/
public Quat4f( Tuple4d t1 )
{
double mag;
mag = 1.0 / Math.sqrt( t1.x * t1.x + t1.y * t1.y + t1.z * t1.z + t1.w
* t1.w );
x = (float) ( t1.x * mag );
y = (float) ( t1.y * mag );
z = (float) ( t1.z * mag );
w = (float) ( t1.w * mag );
}
/**
* Constructs and initializes a Quat4f to (0.0,0.0,0.0,0.0).
*/
public Quat4f()
{
super();
}
/**
* Sets the value of this quaternion to the conjugate of quaternion q1.
*
* @param q1
* the source vector
*/
public final void conjugate( Quat4f q1 )
{
this.x = -q1.x;
this.y = -q1.y;
this.z = -q1.z;
this.w = q1.w;
}
/**
* Sets the value of this quaternion to the conjugate of itself.
*/
public final void conjugate()
{
this.x = -this.x;
this.y = -this.y;
this.z = -this.z;
}
/**
* Sets the value of this quaternion to the quaternion product of
* quaternions q1 and q2 (this = q1 * q2). Note that this is safe for
* aliasing (e.g. this can be q1 or q2).
*
* @param q1
* the first quaternion
* @param q2
* the second quaternion
*/
public final void mul( Quat4f q1, Quat4f q2 )
{
if ( this != q1 && this != q2 )
{
this.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
this.x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
this.y = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
this.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
} else
{
float x, y, w;
w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
y = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
this.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
this.w = w;
this.x = x;
this.y = y;
}
}
/**
* Sets the value of this quaternion to the quaternion product of itself and
* q1 (this = this * q1).
*
* @param q1
* the other quaternion
*/
public final void mul( Quat4f q1 )
{
float x, y, w;
w = this.w * q1.w - this.x * q1.x - this.y * q1.y - this.z * q1.z;
x = this.w * q1.x + q1.w * this.x + this.y * q1.z - this.z * q1.y;
y = this.w * q1.y + q1.w * this.y - this.x * q1.z + this.z * q1.x;
this.z = this.w * q1.z + q1.w * this.z + this.x * q1.y - this.y * q1.x;
this.w = w;
this.x = x;
this.y = y;
}
/**
* Multiplies quaternion q1 by the inverse of quaternion q2 and places the
* value into this quaternion. The value of both argument quaternions is
* preservered (this = q1 * q2^-1).
*
* @param q1
* the first quaternion
* @param q2
* the second quaternion
*/
public final void mulInverse( Quat4f q1, Quat4f q2 )
{
Quat4f tempQuat = new Quat4f( q2 );
tempQuat.inverse();
this.mul( q1, tempQuat );
}
/**
* Multiplies this quaternion by the inverse of quaternion q1 and places the
* value into this quaternion. The value of the argument quaternion is
* preserved (this = this * q^-1).
*
* @param q1
* the other quaternion
*/
public final void mulInverse( Quat4f q1 )
{
Quat4f tempQuat = new Quat4f( q1 );
tempQuat.inverse();
this.mul( tempQuat );
}
/**
* Sets the value of this quaternion to quaternion inverse of quaternion q1.
*
* @param q1
* the quaternion to be inverted
*/
public final void inverse( Quat4f q1 )
{
float norm;
norm = 1.0f / ( q1.w * q1.w + q1.x * q1.x + q1.y * q1.y + q1.z * q1.z );
this.w = norm * q1.w;
this.x = -norm * q1.x;
this.y = -norm * q1.y;
this.z = -norm * q1.z;
}
/**
* Sets the value of this quaternion to the quaternion inverse of itself.
*/
public final void inverse()
{
float norm;
norm = 1.0f / ( this.w * this.w + this.x * this.x + this.y * this.y + this.z
* this.z );
this.w *= norm;
this.x *= -norm;
this.y *= -norm;
this.z *= -norm;
}
/**
* Sets the value of this quaternion to the normalized value of quaternion
* q1.
*
* @param q1
* the quaternion to be normalized.
*/
public final void normalize( Quat4f q1 )
{
float norm;
norm = ( q1.x * q1.x + q1.y * q1.y + q1.z * q1.z + q1.w * q1.w );
if ( norm > 0.0f )
{
norm = 1.0f / (float) Math.sqrt( norm );
this.x = norm * q1.x;
this.y = norm * q1.y;
this.z = norm * q1.z;
this.w = norm * q1.w;
} else
{
this.x = (float) 0.0;
this.y = (float) 0.0;
this.z = (float) 0.0;
this.w = (float) 0.0;
}
}
/**
* Normalizes the value of this quaternion in place.
*/
public final void normalize()
{
float norm;
norm = ( this.x * this.x + this.y * this.y + this.z * this.z + this.w
* this.w );
if ( norm > 0.0f )
{
norm = 1.0f / (float) Math.sqrt( norm );
this.x *= norm;
this.y *= norm;
this.z *= norm;
this.w *= norm;
} else
{
this.x = (float) 0.0;
this.y = (float) 0.0;
this.z = (float) 0.0;
this.w = (float) 0.0;
}
}
/**
* Sets the value of this quaternion to the rotational component of the
* passed matrix.
*
* @param m1
* the Matrix4f
*/
public final void set( Matrix4f m1 )
{
float ww = 0.25f * ( m1.m00 + m1.m11 + m1.m22 + m1.m33 );
if ( ww >= 0 )
{
if ( ww >= EPS2 )
{
this.w = (float) Math.sqrt( (double) ww );
ww = 0.25f / this.w;
this.x = ( m1.m21 - m1.m12 ) * ww;
this.y = ( m1.m02 - m1.m20 ) * ww;
this.z = ( m1.m10 - m1.m01 ) * ww;
return;
}
} else
{
this.w = 0;
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.w = 0;
ww = -0.5f * ( m1.m11 + m1.m22 );
if ( ww >= 0 )
{
if ( ww >= EPS2 )
{
this.x = (float) Math.sqrt( (double) ww );
ww = 1.0f / ( 2.0f * this.x );
this.y = m1.m10 * ww;
this.z = m1.m20 * ww;
return;
}
} else
{
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.x = 0;
ww = 0.5f * ( 1.0f - m1.m22 );
if ( ww >= EPS2 )
{
this.y = (float) Math.sqrt( (double) ww );
this.z = m1.m21 / ( 2.0f * this.y );
return;
}
this.y = 0;
this.z = 1;
}
/**
* Sets the value of this quaternion to the rotational component of the
* passed matrix.
*
* @param m1
* the Matrix4d
*/
public final void set( Matrix4d m1 )
{
double ww = 0.25 * ( m1.m00 + m1.m11 + m1.m22 + m1.m33 );
if ( ww >= 0 )
{
if ( ww >= EPS2 )
{
this.w = (float) Math.sqrt( ww );
ww = 0.25 / this.w;
this.x = (float) ( ( m1.m21 - m1.m12 ) * ww );
this.y = (float) ( ( m1.m02 - m1.m20 ) * ww );
this.z = (float) ( ( m1.m10 - m1.m01 ) * ww );
return;
}
} else
{
this.w = 0;
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.w = 0;
ww = -0.5 * ( m1.m11 + m1.m22 );
if ( ww >= 0 )
{
if ( ww >= EPS2 )
{
this.x = (float) Math.sqrt( ww );
ww = 0.5 / this.x;
this.y = (float) ( m1.m10 * ww );
this.z = (float) ( m1.m20 * ww );
return;
}
} else
{
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.x = 0;
ww = 0.5 * ( 1.0 - m1.m22 );
if ( ww >= EPS2 )
{
this.y = (float) Math.sqrt( ww );
this.z = (float) ( m1.m21 / ( 2.0 * (double) ( this.y ) ) );
return;
}
this.y = 0;
this.z = 1;
}
/**
* Sets the value of this quaternion to the rotational component of the
* passed matrix.
*
* @param m1
* the Matrix3f
*/
public final void set( Matrix3f m1 )
{
float ww = 0.25f * ( m1.m00 + m1.m11 + m1.m22 + 1.0f );
if ( ww >= 0 )
{
if ( ww >= EPS2 )
{
this.w = (float) Math.sqrt( (double) ww );
ww = 0.25f / this.w;
this.x = ( m1.m21 - m1.m12 ) * ww;
this.y = ( m1.m02 - m1.m20 ) * ww;
this.z = ( m1.m10 - m1.m01 ) * ww;
return;
}
} else
{
this.w = 0;
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.w = 0;
ww = -0.5f * ( m1.m11 + m1.m22 );
if ( ww >= 0 )
{
if ( ww >= EPS2 )
{
this.x = (float) Math.sqrt( (double) ww );
ww = 0.5f / this.x;
this.y = m1.m10 * ww;
this.z = m1.m20 * ww;
return;
}
} else
{
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.x = 0;
ww = 0.5f * ( 1.0f - m1.m22 );
if ( ww >= EPS2 )
{
this.y = (float) Math.sqrt( (double) ww );
this.z = m1.m21 / ( 2.0f * this.y );
return;
}
this.y = 0;
this.z = 1;
}
/**
* Sets the value of this quaternion to the rotational component of the
* passed matrix.
*
* @param m1
* the Matrix3d
*/
public final void set( Matrix3d m1 )
{
double ww = 0.25 * ( m1.m00 + m1.m11 + m1.m22 + 1.0f );
if ( ww >= 0 )
{
if ( ww >= EPS2 )
{
this.w = (float) Math.sqrt( ww );
ww = 0.25 / this.w;
this.x = (float) ( ( m1.m21 - m1.m12 ) * ww );
this.y = (float) ( ( m1.m02 - m1.m20 ) * ww );
this.z = (float) ( ( m1.m10 - m1.m01 ) * ww );
return;
}
} else
{
this.w = 0;
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.w = 0;
ww = -0.5 * ( m1.m11 + m1.m22 );
if ( ww >= 0 )
{
if ( ww >= EPS2 )
{
this.x = (float) Math.sqrt( ww );
ww = 0.5 / this.x;
this.y = (float) ( m1.m10 * ww );
this.z = (float) ( m1.m20 * ww );
return;
}
} else
{
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.x = 0;
ww = 0.5 * ( 1.0 - m1.m22 );
if ( ww >= EPS2 )
{
this.y = (float) Math.sqrt( ww );
this.z = (float) ( m1.m21 / ( 2.0 * (double) ( this.y ) ) );
return;
}
this.y = 0;
this.z = 1;
}
/**
* Sets the value of this quaternion to the equivalent rotation of the
* AxisAngle argument.
*
* @param a
* the AxisAngle to be emulated
*/
public final void set( AxisAngle4f a )
{
float mag, amag;
// Quat = cos(theta/2) + sin(theta/2)(roation_axis)
amag = (float) Math.sqrt( a.x * a.x + a.y * a.y + a.z * a.z );
if ( amag < EPS )
{
w = 0.0f;
x = 0.0f;
y = 0.0f;
z = 0.0f;
} else
{
amag = 1.0f / amag;
mag = (float) Math.sin( a.angle / 2.0 );
w = (float) Math.cos( a.angle / 2.0 );
x = a.x * amag * mag;
y = a.y * amag * mag;
z = a.z * amag * mag;
}
}
/**
* Sets the value of this quaternion to the equivalent rotation of the
* AxisAngle argument.
*
* @param a
* the AxisAngle to be emulated
*/
public final void set( AxisAngle4d a )
{
float mag, amag;
// Quat = cos(theta/2) + sin(theta/2)(roation_axis)
amag = (float) ( 1.0 / Math.sqrt( a.x * a.x + a.y * a.y + a.z * a.z ) );
if ( amag < EPS )
{
w = 0.0f;
x = 0.0f;
y = 0.0f;
z = 0.0f;
} else
{
amag = 1.0f / amag;
mag = (float) Math.sin( a.angle / 2.0 );
w = (float) Math.cos( a.angle / 2.0 );
x = (float) a.x * amag * mag;
y = (float) a.y * amag * mag;
z = (float) a.z * amag * mag;
}
}
/**
* Performs a great circle interpolation between this quaternion and the
* quaternion parameter and places the result into this quaternion.
*
* @param q1
* the other quaternion
* @param alpha
* the alpha interpolation parameter
*/
public final void interpolate( Quat4f q1, float alpha )
{
// From "Advanced Animation and Rendering Techniques"
// by Watt and Watt pg. 364, function as implemented appeared to be
// incorrect. Fails to choose the same quaternion for the double
// covering. Resulting in change of direction for rotations.
// Fixed function to negate the first quaternion in the case that the
// dot product of q1 and this is negative. Second case was not needed.
double dot, s1, s2, om, sinom;
dot = x * q1.x + y * q1.y + z * q1.z + w * q1.w;
if ( dot < 0 )
{
// negate quaternion
q1.x = -q1.x;
q1.y = -q1.y;
q1.z = -q1.z;
q1.w = -q1.w;
dot = -dot;
}
if ( ( 1.0 - dot ) > EPS )
{
om = Math.acos( dot );
sinom = Math.sin( om );
s1 = Math.sin( ( 1.0 - alpha ) * om ) / sinom;
s2 = Math.sin( alpha * om ) / sinom;
} else
{
s1 = 1.0 - alpha;
s2 = alpha;
}
w = (float) ( s1 * w + s2 * q1.w );
x = (float) ( s1 * x + s2 * q1.x );
y = (float) ( s1 * y + s2 * q1.y );
z = (float) ( s1 * z + s2 * q1.z );
}
/**
* Performs a great circle interpolation between quaternion q1 and
* quaternion q2 and places the result into this quaternion.
*
* @param q1
* the first quaternion
* @param q2
* the second quaternion
* @param alpha
* the alpha interpolation parameter
*/
public final void interpolate( Quat4f q1, Quat4f q2, float alpha )
{
// From "Advanced Animation and Rendering Techniques"
// by Watt and Watt pg. 364, function as implemented appeared to be
// incorrect. Fails to choose the same quaternion for the double
// covering. Resulting in change of direction for rotations.
// Fixed function to negate the first quaternion in the case that the
// dot product of q1 and this is negative. Second case was not needed.
double dot, s1, s2, om, sinom;
dot = q2.x * q1.x + q2.y * q1.y + q2.z * q1.z + q2.w * q1.w;
if ( dot < 0 )
{
// negate quaternion
q1.x = -q1.x;
q1.y = -q1.y;
q1.z = -q1.z;
q1.w = -q1.w;
dot = -dot;
}
if ( ( 1.0 - dot ) > EPS )
{
om = Math.acos( dot );
sinom = Math.sin( om );
s1 = Math.sin( ( 1.0 - alpha ) * om ) / sinom;
s2 = Math.sin( alpha * om ) / sinom;
} else
{
s1 = 1.0 - alpha;
s2 = alpha;
}
w = (float) ( s1 * q1.w + s2 * q2.w );
x = (float) ( s1 * q1.x + s2 * q2.x );
y = (float) ( s1 * q1.y + s2 * q2.y );
z = (float) ( s1 * q1.z + s2 * q2.z );
}
}