/// <summary>
/// Sets the value of this quaternion to the equivalent rotation
/// of the AxisAngle argument.
/// </summary>
/// <remarks>
/// Sets the value of this quaternion to the equivalent rotation
/// of the AxisAngle argument.
/// </remarks>
/// <param name="a">the AxisAngle to be emulated</param>
public void Set(AxisAngle4d a)
{
double mag;
double amag;
// Quat = cos(theta/2) + sin(theta/2)(roation_axis)
amag = Math.Sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
if (amag < Eps)
{
w = 0.0;
x = 0.0;
y = 0.0;
z = 0.0;
}
else
{
amag = 1.0 / amag;
mag = Math.Sin(a.angle / 2.0);
w = Math.Cos(a.angle / 2.0);
x = a.x * amag * mag;
y = a.y * amag * mag;
z = a.z * amag * mag;
}
}
/// <summary>
/// Sets the value of this matrix to the matrix conversion of the
/// (double precision) axis and angle argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// (double precision) axis and angle argument.
/// </remarks>
/// <param name="a1">the axis and angle to be converted</param>
public void Set(AxisAngle4d a1)
{
double mag = Math.Sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
if (mag < Eps)
{
m00 = 1.0f;
m01 = 0.0f;
m02 = 0.0f;
m10 = 0.0f;
m11 = 1.0f;
m12 = 0.0f;
m20 = 0.0f;
m21 = 0.0f;
m22 = 1.0f;
}
else
{
mag = 1.0 / mag;
double ax = a1.x * mag;
double ay = a1.y * mag;
double az = a1.z * mag;
double sinTheta = Math.Sin(a1.angle);
double cosTheta = Math.Cos(a1.angle);
double t = 1.0 - cosTheta;
double xz = ax * az;
double xy = ax * ay;
double yz = ay * az;
m00 = (float)(t * ax * ax + cosTheta);
m01 = (float)(t * xy - sinTheta * az);
m02 = (float)(t * xz + sinTheta * ay);
m10 = (float)(t * xy + sinTheta * az);
m11 = (float)(t * ay * ay + cosTheta);
m12 = (float)(t * yz - sinTheta * ax);
m20 = (float)(t * xz - sinTheta * ay);
m21 = (float)(t * yz + sinTheta * ax);
m22 = (float)(t * az * az + cosTheta);
}
}
/// <summary>Constructs and initializes an AxisAngle4d from the specified AxisAngle4d.
/// </summary>
/// <remarks>Constructs and initializes an AxisAngle4d from the specified AxisAngle4d.
/// </remarks>
/// <param name="a1">the AxisAngle4d containing the initialization x y z angle data</param>
public AxisAngle4d(AxisAngle4d a1)
{
this.x = a1.x;
this.y = a1.y;
this.z = a1.z;
this.angle = a1.angle;
}
/// <summary>
/// Sets the value of this matrix to the matrix conversion of the
/// double precision axis and angle argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// double precision axis and angle argument.
/// </remarks>
/// <param name="a1">the axis and angle to be converted</param>
public void Set(AxisAngle4d a1)
{
double mag = Math.Sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
if (mag < Eps)
{
m00 = 1.0f;
m01 = 0.0f;
m02 = 0.0f;
m10 = 0.0f;
m11 = 1.0f;
m12 = 0.0f;
m20 = 0.0f;
m21 = 0.0f;
m22 = 1.0f;
}
else
{
mag = 1.0 / mag;
double ax = a1.x * mag;
double ay = a1.y * mag;
double az = a1.z * mag;
float sinTheta = (float)Math.Sin(a1.angle);
float cosTheta = (float)Math.Cos(a1.angle);
float t = 1.0f - cosTheta;
float xz = (float)(ax * az);
float xy = (float)(ax * ay);
float yz = (float)(ay * az);
this.m00 = t * (float)(ax * ax) + cosTheta;
this.m01 = t * xy - sinTheta * (float)az;
this.m02 = t * xz + sinTheta * (float)ay;
this.m10 = t * xy + sinTheta * (float)az;
this.m11 = t * (float)(ay * ay) + cosTheta;
this.m12 = t * yz - sinTheta * (float)ax;
this.m20 = t * xz - sinTheta * (float)ay;
this.m21 = t * yz + sinTheta * (float)ax;
this.m22 = t * (float)(az * az) + cosTheta;
}
this.m03 = 0.0f;
this.m13 = 0.0f;
this.m23 = 0.0f;
this.m30 = 0.0f;
this.m31 = 0.0f;
this.m32 = 0.0f;
this.m33 = 1.0f;
}
/// <summary>
/// Returns true if all of the data members of AxisAngle4d a1 are
/// equal to the corresponding data members in this AxisAngle4d.
/// </summary>
/// <remarks>
/// Returns true if all of the data members of AxisAngle4d a1 are
/// equal to the corresponding data members in this AxisAngle4d.
/// </remarks>
/// <param name="a1">the axis-angle with which the comparison is made</param>
/// <returns>true or false</returns>
public virtual bool Equals(AxisAngle4d a1)
{
try
{
return (this.x == a1.x && this.y == a1.y && this.z == a1.z && this.angle == a1.angle
);
}
catch (ArgumentNullException)
{
return false;
}
}
/// <summary>
/// Returns true if the L-infinite distance between this axis-angle
/// and axis-angle a1 is less than or equal to the epsilon parameter,
/// otherwise returns false.
/// </summary>
/// <remarks>
/// Returns true if the L-infinite distance between this axis-angle
/// and axis-angle a1 is less than or equal to the epsilon parameter,
/// otherwise returns false. The L-infinite
/// distance is equal to
/// MAX[abs(x1-x2), abs(y1-y2), abs(z1-z2), abs(angle1-angle2)].
/// </remarks>
/// <param name="a1">the axis-angle to be compared to this axis-angle</param>
/// <param name="epsilon">the threshold value</param>
public virtual bool EpsilonEquals(AxisAngle4d a1, double epsilon)
{
double diff;
diff = x - a1.x;
if ((diff < 0 ? -diff : diff) > epsilon)
{
return false;
}
diff = y - a1.y;
if ((diff < 0 ? -diff : diff) > epsilon)
{
return false;
}
diff = z - a1.z;
if ((diff < 0 ? -diff : diff) > epsilon)
{
return false;
}
diff = angle - a1.angle;
if ((diff < 0 ? -diff : diff) > epsilon)
{
return false;
}
return true;
}
/// <summary>Constructs and initializes an AxisAngle4f from the specified AxisAngle4d.
/// </summary>
/// <remarks>Constructs and initializes an AxisAngle4f from the specified AxisAngle4d.
/// </remarks>
/// <param name="a1">the AxisAngle4d containing the initialization x y z angle data</param>
public AxisAngle4f(AxisAngle4d a1)
{
this.x = (float)a1.x;
this.y = (float)a1.y;
this.z = (float)a1.z;
this.angle = (float)a1.angle;
}
/// <summary>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix equivalent values of the axis-angle argument; the other
/// elements of this matrix are unchanged; a singular value
/// decomposition is performed on this object's upper 3x3 matrix to
/// factor out the scale, then this object's upper 3x3 matrix components
/// are replaced by the matrix equivalent of the axis-angle,
/// and then the scale is reapplied to the rotational components.
/// </summary>
/// <remarks>
/// Sets the rotational component (upper 3x3) of this matrix to the
/// matrix equivalent values of the axis-angle argument; the other
/// elements of this matrix are unchanged; a singular value
/// decomposition is performed on this object's upper 3x3 matrix to
/// factor out the scale, then this object's upper 3x3 matrix components
/// are replaced by the matrix equivalent of the axis-angle,
/// and then the scale is reapplied to the rotational components.
/// </remarks>
/// <param name="a1">the axis-angle to be converted (x, y, z, angle)</param>
public void SetRotation(AxisAngle4d a1)
{
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
GetScaleRotate(tmp_scale, tmp_rot);
double mag = 1.0 / Math.Sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
double ax = a1.x * mag;
double ay = a1.y * mag;
double az = a1.z * mag;
double sinTheta = Math.Sin(a1.angle);
double cosTheta = Math.Cos(a1.angle);
double t = 1.0 - cosTheta;
double xz = a1.x * a1.z;
double xy = a1.x * a1.y;
double yz = a1.y * a1.z;
m00 = (t * ax * ax + cosTheta) * tmp_scale[0];
m01 = (t * xy - sinTheta * az) * tmp_scale[1];
m02 = (t * xz + sinTheta * ay) * tmp_scale[2];
m10 = (t * xy + sinTheta * az) * tmp_scale[0];
m11 = (t * ay * ay + cosTheta) * tmp_scale[1];
m12 = (t * yz - sinTheta * ax) * tmp_scale[2];
m20 = (t * xz - sinTheta * ay) * tmp_scale[0];
m21 = (t * yz + sinTheta * ax) * tmp_scale[1];
m22 = (t * az * az + cosTheta) * tmp_scale[2];
}
/// <summary>
/// Sets the value of this matrix to the matrix conversion of the
/// double precision axis and angle argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// double precision axis and angle argument.
/// </remarks>
/// <param name="a1">the axis and angle to be converted</param>
public void Set(AxisAngle4d a1)
{
double mag = Math.Sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
if (mag < Eps)
{
m00 = 1.0;
m01 = 0.0;
m02 = 0.0;
m10 = 0.0;
m11 = 1.0;
m12 = 0.0;
m20 = 0.0;
m21 = 0.0;
m22 = 1.0;
}
else
{
mag = 1.0 / mag;
double ax = a1.x * mag;
double ay = a1.y * mag;
double az = a1.z * mag;
double sinTheta = Math.Sin(a1.angle);
double cosTheta = Math.Cos(a1.angle);
double t = 1.0 - cosTheta;
double xz = ax * az;
double xy = ax * ay;
double yz = ay * az;
m00 = t * ax * ax + cosTheta;
m01 = t * xy - sinTheta * az;
m02 = t * xz + sinTheta * ay;
m10 = t * xy + sinTheta * az;
m11 = t * ay * ay + cosTheta;
m12 = t * yz - sinTheta * ax;
m20 = t * xz - sinTheta * ay;
m21 = t * yz + sinTheta * ax;
m22 = t * az * az + cosTheta;
}
m03 = 0.0;
m13 = 0.0;
m23 = 0.0;
m30 = 0.0;
m31 = 0.0;
m32 = 0.0;
m33 = 1.0;
}
