/// <summary>
/// Sets the value of this matrix to the matrix conversion of the
/// (single precision) axis and angle argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// (single precision) axis and angle argument.
/// </remarks>
/// <param name="a1">the axis and angle to be converted</param>
public void Set(AxisAngle4f a1)
{
float mag = (float)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.0f / mag;
float ax = a1.x * mag;
float ay = a1.y * mag;
float az = a1.z * mag;
float sinTheta = (float)Math.Sin((float)a1.angle);
float cosTheta = (float)Math.Cos((float)a1.angle);
float t = (float)1.0 - cosTheta;
float xz = ax * az;
float xy = ax * ay;
float 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;
}
}
/// <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(AxisAngle4f a1)
{
double[] tmp_rot = new double[9];
// scratch matrix
double[] tmp_scale = new double[3];
// scratch matrix
GetScaleRotate(tmp_scale, tmp_rot);
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 = a1.x * a1.z;
double xy = a1.x * a1.y;
double yz = a1.y * a1.z;
m00 = (float)((t * ax * ax + cosTheta) * tmp_scale[0]);
m01 = (float)((t * xy - sinTheta * az) * tmp_scale[1]);
m02 = (float)((t * xz + sinTheta * ay) * tmp_scale[2]);
m10 = (float)((t * xy + sinTheta * az) * tmp_scale[0]);
m11 = (float)((t * ay * ay + cosTheta) * tmp_scale[1]);
m12 = (float)((t * yz - sinTheta * ax) * tmp_scale[2]);
m20 = (float)((t * xz - sinTheta * ay) * tmp_scale[0]);
m21 = (float)((t * yz + sinTheta * ax) * tmp_scale[1]);
m22 = (float)((t * az * az + cosTheta) * tmp_scale[2]);
}
}
/// <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(AxisAngle4f 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
{
mag = Math.Sin(a.angle / 2.0);
amag = 1.0 / amag;
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
/// single precision axis and angle argument.
/// </summary>
/// <remarks>
/// Sets the value of this matrix to the matrix conversion of the
/// single precision axis and angle argument.
/// </remarks>
/// <param name="a1">the axis and angle to be converted</param>
public void Set(AxisAngle4f 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;
}
}
/// <summary>Sets the value of this axis angle to the value of axis angle a1.</summary>
/// <remarks>Sets the value of this axis angle to the value of axis angle a1.</remarks>
/// <param name="a1">the axis angle to be copied</param>
public void Set(AxisAngle4f a1)
{
this.x = a1.x;
this.y = a1.y;
this.z = a1.z;
this.angle = a1.angle;
}
/// <summary>
/// Constructs and initializes an AxisAngle4d from the specified
/// AxisAngle4f.
/// </summary>
/// <remarks>
/// Constructs and initializes an AxisAngle4d from the specified
/// AxisAngle4f.
/// </remarks>
/// <param name="a1">the AxisAngle4f containing the initialization x y z angle data</param>
public AxisAngle4d(AxisAngle4f a1)
{
this.x = a1.x;
this.y = a1.y;
this.z = a1.z;
this.angle = a1.angle;
}
/// <summary>
/// Returns true if all of the data members of AxisAngle4f a1 are
/// equal to the corresponding data members in this AxisAngle4f.
/// </summary>
/// <remarks>
/// Returns true if all of the data members of AxisAngle4f a1 are
/// equal to the corresponding data members in this AxisAngle4f.
/// </remarks>
/// <param name="a1">the axis-angle with which the comparison is made</param>
/// <returns>true or false</returns>
public virtual bool Equals(AxisAngle4f 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(AxisAngle4f a1, float epsilon)
{
float 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;
}
